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ABSTRACT 


This  research  describes  one  of  several  alternatives  being  explored 
to  establish  advanced  guidance  techniques  for  the  Army's  long  range 
tactical  missile,  the  PERSHING.  The  analysis  is  given  for  estimating 
the  inertial  measurement  unit's  error  states  in  an  aided  terminal 
guidance  mode.  Position  observations  measured  by  a  radar  area  correla¬ 
tion  system  and  a  radar  altimeter  are  processed  in  an  extended  Kalman 
filter  to  yield  the  suboptimal  estimates  of  the  inertial  measurement 
unit's  errors. 

The  dynamics  of  the  inertial  system  are  modeled  in  four  coordinate 
systems  to  Allow  the  choice  of  the  least  complex  mechanization.  To 
minimize  on-board  computer  requirements,  an  analysis  of  the  filter's 
performance  is  made  by  comparing  the  optimal  filter  grins  with  a  filter 
formulated  with  fixed  gain A  chosen  a  posteriori  from  those  computed 
optimally . 

Several  variations  of  the  problem  are  simulated.  The  observations 
are  modeled  as  discrete  time  measurements  obtained  once  per  second  and 
then  once  every  two  seconds  in  the  terminal  guidance  phase  oc  the 
flight.  Also,  conditions  are  varied  to  simulate  optimistic  as  well  as 
realistic  radar  imagery  processing  time  and  its  effect  on  the  accuracy 
of  the  filter. 

Results  indicating  filter  error  and  root  mean  square  values  of  the 

state  estimates  obtained  in  a  Monte  Carlo  computer  simulation  are  shown 
graphically  to  validate  the  conclusions . 
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CHAPTER  I .  INTRODUCTION 


1.1  BACKGROUND 

Studies  are  being  conducted  by  various  elements  of  the  U.S.  Army 
Missile  Command  and  teams  of  radar  and  guidance  contractors  to  estab¬ 
lish  advanced  guidance  and  reentry  techniques  for  the  Army's  tactical 
ballistic  missile,  the  PERSHING.  These  wide  ranging  studies  are 
considering  the  feasibility  of  modifying  the  reentry  body  configuration 
and  guidance  trajectories,  updating  the  inertial  sensors,  digitizing 
the  control  system,  changing  the  vehicle's  aerodynamic  characteristics, 
and  augmenting  the  guidance  system.  The  objective  of  these  studies  and 
follow-up  development  work  is  to  improve  the  missile's  basic  figure  of 
merit  which  is  the  target  miss  distance  at  impact. 

This  report  summarizes  a  study  of  only  one  of  those  alternatives, 
in  particular,  the  analysis  and  simulation  is  performed  for  an  inertial 
guidance  system  augmented  by  *  radar  area  correlation  device.  Aiding 
an  inertial  measurement  unit  (IMU)  with  position  and  velocity  measuring 
devices  is  not  a  new  idea .  The  literature  abounds  with  techniques  and 
methods  of  maintaining  inertial  equipment  within  acceptable  error 
bounds.  Papers  by  Broxmeyer  [1],  Duncan  [2],  Dworetsky  and  Edwards  [3], 
and  Friedman  [4]  are  only  a  few  which  are  representative  of  the  early 
work  in  this  area.  However,  the  use  of  additional  external  measure¬ 
ments,  optimal  in  a  sense,  is  much  more  recent  and  has  never  been 
applied  to  the  PERSHING. 
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1.2  PREVIOUS  INVESTIGATIONS 

Of  the  many  attempts  to  improve  inertial  system  performance  by 
using  external  information,  one  of  the  earliest  was  that  of  damping  the 
84-minute  and  24-hour  natural  periods  of  the  vertical  and  gyrocompass 
heading  loops,  respectively.  The  primary  purpose  of  damping  the 
inertial  system  was  to  reduce  the  amplitude  of  the  oscillations  caused 
by  offsets  and  gyro  drifts,  or  at  worst,  to  reduce  the  oscillations  to 
a  fixed  constant  value.  Attention  was  focused  on  various  damping  con¬ 
figurations  or  equalizers  which  led  to  concepts  such  as  second-order 
(velocity)  and  third-order  (acceleration)  tuning  [3] .  The  need  for  an 
external  source  in  damping  the  IMU  was  apparent  when  it  was  observed 
that  errors  caused  by  vehicle  motion  would  result  if  only  information 
from  the  inertial  system  were  used  [5] .  However,  if  external  speed 
information  was  properly  introduced  into  the  inertial  system,  there 
would  be  no  error  caused  by  the  vehicular  induced  motion  provided  that 
the  external  information  which  was  used  matched  the  inertially  derived 
information  in  accuracy. 

Another  way  of  using  external  information  to  obviate  inertial 
errors  was  found  not  in  the  literature  but  in  practice.  In  that 
method  external  measurements  were  used  directly  to  update  the  inertial 
system  rather  than  implement  a  damping  scheme.  Inertial  system  posi¬ 
tion  indication  was  changed  to  agree  with  the  results  of  a  position 
fix  and  inertial  system  velocity  indication  was  changed  to  agree  with 
the  results  of  a  velocity  measurement  update.  Although  it  may 
have  been  expedient,  this  approach  ignored  the  fact  that  the  inertial 


-2 


system  errors  were  primarily  caused  by  random  time  varying  inertial 
sensor  errors  and  that  the  external  measurements  also  contained  random 
errors  which  may  have  been  significant  compared  to  the  inertial  system 
errors . 

The  use,  by  practicing  navigators,  of  measurement  updates  in  this 
manner,  however,  led  systems  designers  to  consider  more  viable  alter¬ 
natives  in  using  the  external  fix  information  when  it  became  available. 
Consequently,  within  the  framework  of  inertial  systems  analysis,  the 
problem  evolved  of  finding  the  optimal  estimate  of  the  system  error 
(a  random  variable)  when  a  linear  function  of  that  variable  was 
corrupted  by  additive  noise. 

The  earliest  published  study  of  this  class  of  problems  (1809)  was 
Gauss's  Theoria  Motes  Corporum  Coelestium  in  which  astronomical 
parameters  were  estimated.  Legendre  independently  invented  the  method 
of  least  squares  estimation  and  published  it  in  1806.  (According  to 
Sorenson  [6],  Gauss  claimed  to  have  invented  the  method  of  least 
squares  in  1795  but  did  not  publish  it  until  1809.)  R.  A.  Fisher 
introduced  the  maximum  likelihood  method  in  1912.  In  1942,  Kolomogorov 
and  Weiner  independently  developed  a  linear  minimum  mean-square  estima¬ 
tion  technique.  The  key  result  of  these  studies  was  an  integral 
equation  called  the  Weiner-Hopf  equation  in  the  U.S.  The  solution  of 
this  equation  was  a  weighting  function  which,  when  convolved  with  the 
corrupted  linear  measurement,  produced  the  unbiased  minimum  variance 
estimate  of  the  random  signal.  The  application  was  limited  initially 
to  statistically  stationary  processes  and  provided  optimum  estimates 
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only  in  the  steady  state.  Kolomogorov  and  Weiner's  work  was  expanded 
during  the  next  20  years  to  include  discrete,  nonstationary,  and  multi¬ 
port  systems,  but  in  a  way  which  required  cumbersome  calculations.  In 
the  1950's,  the  idea  of  generating  least-squares  estimates  recursively 
was  introduced  by  several  investigators:  Carlton  and  Follin;  Swerling; 
Blum,  Robbins,  and  Mundo;  and  Kiefer  and  Wolfowitz,  all  noted  in 
Sorensen's  paper  [6] .  Kalman  [7]  (1960)  and  Kalman  and  Bucy  [8]  (1961) 
generalized  the  results  of  Weiner  and  Kolomogorov  to  nonstationary 
random  processes  and  developed  the  problem  based  on  a  state-space  and 
time-domain  formulation.  The  recursive  nature  of  the  filter  developed 
by  Kalman  made  it  ideally  suitable  for  solutions  on  the  digital 
computer . 

Since  then,  there  has  been  a  veritable  explosion  of  investigations 
applying  the  estimation  technique,  commonly  called  the  Kalman  filter, 
to  a  host  of  aerospace  oriented  problems.  The  study  by  Gelb  and 
Sutherland  [ 9]  alone  lists  over  40  such  references.  Yet,  in  all  of 
this  literature,  there  was  interestingly  enough  no  unclassified 
reference  to  the  particular  optimal  combination  of  systems  described  in 
this  report,  i.e.,  radar  area  correlation  and  inertial  guidance 
systems . 

1.3  PROBLEM  MOTIVATION 

In  this  application,  the  IMU  on  the  PERSHING  must  perform  two 
functions:  (1)  provide  data  to  allow  the  missile  to  be  guided  during 
boost  accurately  enough  to  reach  a  terminal  acquisition  basket  during 
reentry  and  (2)  provide  the  attitude  reference  for  the  terminal 
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guidance  device.  Prior  to  the  terminal  guidance  phase,  the  reentry 
body  follows  a  ballistic  trajectory  determined  by  the  launch-phase 
guidance  system.  At  a  particular  altitude,  in  the  case  of  PERSHING 
30,000  feet,  the  reentry  body  must  be  in  a  specified  position  if  the 
terminal  guidance  scheme  is  to  be  of  any  value.  Without  terminal 
guidance  th*e  reentry  body  would  continue  on  its  ballistic  path  into 
impact  with  a  miss  distance  representing  the  accumulation  of  all 
errors  of  the  launch  phase,  mid-course  i>hase,  and  terminal  phase  of 
the  flight.  This  covers  the  entire  range  of  possible  error  sources 
including  atmospheric  perturbations  on  the  intended  trajectory,  errors 
in  the  guidance  and  propulsion  systems,  false  targeting,  etc.  To 
minimize  the  effects  of  these  errors  and  consequently  the  miss 
distance,  use  of  a  radar  area  correlation  terminal  guidance  system  will 
be  studied. 

The  map-matching  function,  as  it  is  sometimes  known,  is  to  deter¬ 
mine  the  difference  between  the  desired  position  and  actual  position  of 
the  vehicle  at  various  altitudes  by  use  of  radar  correlation  detectors. 
Knowledge  of  position  error  is  sufficient  to  determine  the  necessary 
trajectory  correction  because  the  velocity  vector  of  the  reentry  body, 
within  certain  bounds,  is  precisely  known  (as  determined  by  the  IMU)  . 

To  obtain  the  position  error,  an  image  of  an  area  near  or  ideally 
including  the  target,  is  obtained  at  a  specified  altitude  using  a  side¬ 
looking  airborne  radar.  This  area  is  mapped  by  a  beam  scan.  The  radar 
image  obtained  is  compared  with  previously  stored  reference  imagery  to 
determine  the  position  error.  To  accomplish  this  objective,  in  the 
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presence  of  errors  in  the  image  formation  and  radar  noise,  the  images 
are  matched  for  that  displacement  which  results  in  the  highest  correla¬ 
tion  between  radar  and  reference; 

Conceptually,  this  external  data  could  be  used  to  update  the 
inertial  system  because  the  position  and  velocity  indicated  by  the  IMU 
would  be  in  error.  Unfortunately,  these  data  are  too  imperfect  to  be 
used  directly  because  of  measurement  errors  and  other  effects  such  as 
intensity  quantizing  errors,  scale  factor  errors,  resolution  effects, 
etc.  Thus  the  discussion  returns  to  the  general  class  of  problems 
described  earlier,  i.e.,  given  measurements  Z^,  Z^,  • • *zn>  determine 
the  best  estimate  of  the  states  X^,  X^,  . ..X^.  With  certain  restric¬ 
tions,  criteria  that  define  the  optimal  state  estimate  introduce 
the  formulation  commonly  referred  to  as  the  Kalman  filter. 

1.4  PROBLEM  STATEMENT 

The  problem  is  to  optimally  estimate  the  inertial  guidance  system 
errors  using  the  position  measurements  obtained  from  a  radar  area 
correlation  system  and  a  radar  altimeter  during  the  terminal  phase  of 
the  PERSHING  missile  flight  and  to  consider  suboptimal  mechanizations 
which  would  simplify  the  hardware. 

1.5  OBJECTIVES 

There  are  several  objectives  to  be  fulfilled  in  the  course  of 
seeking  a  solution  to  the  problem.  Representation  or  modeling  of  the 
inertial  system  will  require  a  decision  on  the  mechanization  to  be 
used,  i.e.,  wandering  azimuth,  local  level  north-east,  tangent  plane  or 
space-fixed  tangent  plane  mechanizations. 


There  is  time  enough  in  the  terminal  guidance  phase  to  permit  as 
few  as  5  or  as  many  as  15  observations  of  position.  The  second  objec¬ 
tive  will  be  to  determine  the  optimum  number  of  measurement  observa¬ 
tions  to  be  made  by  the  radar  system  and  their  spatial  timing. 

It  is  conceivable  that  an  on-board  digital  computer  will  be 
utilized  to  do  a  variety  of  command  and  control  tastes.  To  simplify  its 
computational  burden,  the  third  objective  will  be  to  discard  systemati¬ 
cally  error  variables  from  the  complete  mathematical  model  for  a  mini¬ 
mum  Kalman  filter  mechanization. 

The  optimal  gains  applied  to  the  updating  of  the  state  variable 
estimate  are  typically  time  varying.  The  fourth  objective,  consistent 
with  reducing  the  computation  burden,  will  be  to  determine  the  extent 
fixed  gains  or  other  simplifications  such  as  programmed  gains  can  be 
utilized  in  the  model. 

The  fifth  objective  is  to  define  a  set  of  specifications  on  the 
IMU  and  radar  sys'ems  suitable  for  real  world  system  synthesis. 


CHAPTER  II.  DYNAMICS  OF  THE  INERTIAL  SYSTEM 


2.1  GENERAL 

Detailed  descriptions  of  inertial  sensors  and  system’s  are  avail¬ 
able.  in  standard  texts  by  O'Donnell  [10],  Pitman  [11],  and  Fernandez 
and  Maconiber  [12].  It  is  assumed  that  the  reader  is  generally  knowl¬ 
edgeable  with  their  works.  For  completeness  however,  the  following 
paragraph  summarizes  the  essentials. 

The  inertial  guidance  system  consists  of  sensors  that  measure 
specific  force  (the  accelerometers)  and  sensors  that  measure  angular 
motion  in  a  coordinate  system  fixed  in  the  IMU  (the  gyros).  The 
gimballed  platform,  shown  schematically  in  Fig.  2.1.  is  typical  of 
those  that  have  been  used  on  the  PERSHING.  It  permits  isolation  of  the 
instruments  from  the  angular  motion  of  the  vehicle  by  using  the  gyros 
as  sensors  of  orientation  change.  Through  gimbal  servos,  the  platform 
is  returned  to  its  proper  attitude  permitting  the  accelerometers  to 
measure  changes  in  specific  force.  To  obtain  velocity ,  position,  and 
attitude  information  from  the  instruments  and  the  platform,  sets  of 
equations  are  mechanized  in  the  computer.  Additionally,  the  mechaniza¬ 
tion  equations  provide  the  information  for  torques  needed  to  precess 
the  gyros. 

In  the  sections  which  follow,  mechanization  equations  will  first 
be  developed  in  ideal  invariant  vector  form.  Because  system-state 
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FIG.  2.1.  SCHEMATIC  DIAGRAM  OF'  A  THREE  GIMBALLED  INERTIAL 
GUIDANCE  SYSTEM'S  IMU 


variables  are  sought  for  use  in  the  estimation  problem,  inertial  system 
error  equations  will  be  developed  as  well  as  a  comment  on  their  appli¬ 
cability  in  the  problem. 

2.2  INVARIANT  VECTOR  FORMULATION  OF  THE  IMU  MECHANIZATION  EQUATIONS 

It  is  meaningful  at  this  point  to  briefly  explain  the  differences 
among  the  various  coordinate  bases  which  will  be  used  in  the  develop¬ 
ment  and  indicate  why  one  may  be  preferred  over  the  other.  In  deriving 
the  ideal  mechanization  equations  in  text  (and  the  error  equations  in 
the  appendices),  the  following  bases  are  important: 

(I)  -  The  inertial  basis  defined  as  fixed  in  "space"  and  nonmoving 


(E)  -  The  earth  fixed  basis  which  rotates  with  respect  to  the 
inertial  basis  at  earth's  angular  velocity 


(L)  -  The  local  basis  defined  by  the  true  position  on  the  earth 

(C)  -  The  computer  basis  defined  to  be  the  same  as  the  local  basis 
except  it  also  is  defined  by  the  indicated  position 

(P)  -  The  platform  basis  defined  as  equal  to  the  computer  basis 
but  rotated  from  it  by  some  small  angle. 

It  is  desirable  to  derive  the  equations  in  the  local  basis  because 

navigation  is  done  in  the  basis  which  is  defined  by  the  true  position 

of  the  vehicle  on  the  earth.  In  the  historical  development  of  the  art 

of  dead  reckoning  navigation,  the  local  level  or  "plumb  bob"  level 

(described  later)  was  the  most  easily  realizable  vertical.  Thus  the 

local  basis,  in  many  cases,  tends  to  be  similary  mechanized,  i.e., 

locally  level.  That  practice  will  be  followed  in  the  sequel. 

The  computer  basis,  used  in  the  development  of  the  error 
equations,  is  the  basis  in  which  the  navigation  variables  are  computed 
and  output.  Thus,  this  is  the  basis  in  which  indicated  position  is 
given.  It  is  also  convenient  to  think  of  it  as  the  realizable  mechani¬ 
zation  of  the  local  basis. 

The  platform  basis,  used  in  the  error  equations,  is  the  basis  in 
which  the  inertial  instruments  are  considered  fixed.  It  is  in  error 
with  the  computer  basis  in  an  amount  contributed  by  the  gyro  and 
accelerometer  instrument  errors  as  well  as  several  other  error  sources. 

These  bases  are  shown  in  Fig.  2.2  which  defines  their  relative 
orientations.  The  mass  center  of  the  earth,  represented  by  0,  is 

■4  -4  — > 

also  the  center  of  the  inertially  fixed  basis,  X^.,  Y^,  2^.  The 
subscripts  E  and  L  refer  to  unit  vectors  fixed  in  the  rotating  earth's 
basis  and  local  basis,  respectively*. 

*J.  R.  Streeter  discusses  using  the  center  of  the  earth  rather  than  the 
sun  as  the  origin  of  coordinates  fixed  in  inertial  space  in 
O'Donnell's  book  [ 10J  . 
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FIG.  2.2.  COORDINATE  SYSTEM  ORIENTATIONS 


The  following  notations  are  in  the  development  which  follow: 


vector  expressing  position  from  earth's  center 
of  mass  to  the  vehicle 

vector  expressing  velocity  of  vehicle  with 
respect  to  earth  fixed  basis 

vector  expressing  velocity  of  vehicle  with 
respect  to  an  inertially  fixed  basis  which  is 
nominally  time  invariant 

vector  expressing  velocity  of  vehicle  with 
respect  to  a  local  basis. 


Also  let 

E 

VAr 

and 

U4?  =  V  +  T^”1  X  t 


-11- 


By  the  Coriolis  law, 


L  £ 

-»  -»  ->E-L  -» 

r  =  r  +  co  x  r 

=  .  (2.1) 


Also, 


I 

-» 

r 


h  ->E-I  -> 

r  +  co  x  r 


»  V  + 


E-I 

CO 


x  r 


(2.2) 


The  specific  force  meter  (SFM),  as  described  by  Markey  and  Hovorka 


(13),  can  be  idealized  as  follows: 


II 

r  =  f  +  G 


(2.3) 


where 


-> 

f  A  specific  force 


G 


A 


II 

r 


.2  .  , 

d  tr) 

dt 


A 

I 


gravitational  field  intensify  vector  at  the  center 
of  mass  of  the  SFM  mass  element 

vector  expressing  the  acceleration  of  the  specific 
force  meter's  case  with  respect  to  the  inertial 
basis . 


The  left  side  of  Eq.  (2.3)  is  given  as 


n  i 

r  =  U  ,  (2.4) 


Substituting  Eq.  (2.2)  into  Eq.  (2.3)  and  noting  co  ,  the  earth's 
spin  rate,  is  a  constant  for  practical  purposes. 


f  +  G 


_ai(* 

-  dt  vv 


-»E- 
+  w 


7?  -»E-I  v  i 
1  v  X  r 


„  ->E-I  -» 

v  +w  XU 


L 

v  ,  -^L-I  7?  ->E-I  -* 

“  v  +  id  xV+cj  XU 


(2.5) 


Substituting  Eq.  (2.2)  into  Eq.  (2.5)  for  u  gives 

.  (2.6> 

Combining  and  rearranging  yields 

v.f+?-(;,!-I+»l'I)xv-»E-Ix(»E-Ix?)  .  (2.2) 

where  the  gravity  field  intensity  vector  can  now  be  defined  as 


3  A 


->  -*E-I 
G  -  to 


“A 

v  u  x  r  / 


This  is  the  vector  which  is  the  vector  sum  of  the  gravitational  field 
intensity  vector  and  the  centrifugal  acceleration  vector  caused  by  the 
earth's  rotation  relative  to  an  inertial  basis.  More  commonly,  it  is 
the  apparent  specific  force  caused  by  gravity  which  acts  along  a  plumb 
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bob  suspended  to  the  point  considered  (Fig.  2.3).  This  is  an  important 

consideration  because  most  o£  the  tactical  maps  in  use  today  arc  based 

on  the  concept  of  a  local,  plumb  bob  level.  Even  though  more  and  more 

use  is  made  of  cartographic  satellites  whiph  map  earth's  imagery 
— > 

relative  to  the  G  gravity,  the  former  is  still  more  commonly  used  by 
the  Army.  Thus,  Eq.  (2.7)  simplifies  to  the  following  ideal  invariant 
vector  form: 


(2.8a) 


Rewriting  Eq.  (2.1)  for  completeness, 


(2.8b) 


FIG.  2.3.  GEOCENTRIC  AND  MASS  ATTRACTION  PARAMETERS 
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Equations  (2.8a)  and  (2.8b)  are  the  ideal  position  state  equations 
in  convenient  form  for  hardware  implementation.  The  left  side  of 
Eq.  (2.8a)  is  the  derivative  of  velocity  relative  to  a  local  basis 
oriented  near  the  earth's  surface.  The  term  f,  the  specific  force,  is 
provided  directly  by  the  accelerometers,  and  the  last  two  terms  are 
calculated  from  knowledge  of  position  and  the  angular  velocity  of  the 
local  basis  which  are  instrumented  and  computed  on-board  the  vehicle. 

It  remains  to  choose  the  local  basis  for  coordinatizing  Eq.  (2.8a) 
explicitly.  Several  are  common,  including  the  wander  azimuth,  local 
level  north-east,  tangent  plane,  space-fixed  tangent  plane,  free 
azimuth,  latitude  longitude,  and  relocated  pole  latitude  longitude. 
These  various  mechanizations  differ  basically  in  the  way  the  vector 
wL"’1  is  prescribed. 

For  purposes  of  this  study,  the  free  azimuth,  the  tangent  plane, 
the  space- fixed  tangent  plane,  and  the  local  level  north-east  mechani¬ 
zations  will  be  investigated. 

2.3  THE  IMU  ERROR  EQUATIONS 

Ideally,  Eq.  (2.8a)  may  be  implemented  in  any  of  the  coordinate 
systems  previously  mentioned.  Realistically  however,  it  is  impossible 
to  instrument  the  equations  without  errors  because  of  such  factors  as 
gyro  drift,  erroneous  gyro  and  accelerometer  scale  factors,  accelerom¬ 
eter  bias,  etc.  Consequently  for  reasons  that  will  be  discussed  later, 
the  ideal  equations  will  not  be  used  in  the  filter.  Instead,  standard 
perturbation  techniques  will  be  applied  so  that  the  effects  of  the 
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errors  on  the  navigation  computations  can  be  determined.  The  inertial 
system  errors  are  estimated  in  the  filter  and  not  the  states  of  the 
vehicle  nor  of  the  navigation  problem  directly. 

The  inertial  system  errors  are  described  by  the  following  error 
vector  differential  equations: 

li 

t  =  -  ijL‘Ix  V  +  K  •  wL’T  +  "e  (platform  error)  (2.9) 

s 

and 

pr  +  -  3  r  rl*  61?=  Kf  •  ~£  +  b  -  x  f  (position  error)  (2.10) 

where 

— > 

+•  A  vector  representing  the  small  angular  misalignment 

between  a  basis  fixed  in  the  computer  and  a  basis  fixed 
in  the  platform  (Fig.  2.4) 

ws  A  Schuler  angular  frequency  given  by  J K/r^ 

^  ^  tensor  representing  the  gyro  scale  factor  errors  on  the 
8  ' *  principal  diagonal  and  misalignments  on  the  off-diagonal 

K£  A  tensor  representing  the  SFM  scale  factor  errors  on  the 

principal  diagonal  and  misalignments  on  the  off-diagonal 

,  A  vector  representing  gyro-drift  rate  as  an  error 

b  A  vector  representing  SFM  bias  as  an  error 

r  r  A  dyad  of  unit  vectors  in  the  r  direction. 

Equations  (2.9)  and  (2.10)  are  derived  in  Appendix  A  in  a  manner 

following  that  of  Lange  [14].  It  differs  from  derivations  shown  in 

the  standard  texts  mentioned  earlier. 
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PIG.  2.4.  EULER  ANGLE  ROTATIONS  OF  PLATFORM  ERROR  ANGLES,  ♦ 
Expanding  Eq.  (2.10)  by  the  Coriolis  Law  yields 

s^+^X  8? 


nd 


3  3  -»l-t  H  ^L-I  -» 

5r  =  &r  +  x  5r  +  w  x  &r 


+  51-1  X  (  or+u1"1  x  8?)  . 

II 

Substituting  Eq.  (2.12)  into  Eq.  (2.10)  for  5r  gives 

LL  Kt  T  — +  — >L" I  I  V,  Cy  I 

f,?— 2b1'1  X  8?-«L  I  X  8?-<s  X^s,  X  8t/ 


-,,2  f?-  1  r  rl  •  5T  +  K.  •  2  +  *P'°X  £  •  <2,13) 


The  results  shown  inEqs.  (2.9)  and  (2.13)  are  important.  They  imply 
— ► 

that  if  \}r  is  used  as  a  basic  characterization  of  platform  angles,  a 

differential  equation  exists  for  which  is  independent  of  position 

— > 

errors.  Thus,  the.  decoupled  equation,  Eq.  (2.9),  can  be  solved  inde¬ 
pendently  and  the  result  used  as  driving  variables  in  the  position 
error  equation,  Eq.  (2.13). 

Now  coordinatize  the  two  sets  of  equations  into  a  local  basis 
(L-basis) .  Beginning  with  Eq.  (2.9)  and  noting  vector  and  dyadic 
operators, 


K 

8 

L 


*KSl 

m12 

m13 

m21 

AKg2 

m23  » 

(2.16) 

m31 

m32 

AK  _ 

g3 

(2.17) 
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Thus  Eq.  (2.9),  the  platform  error  angles,  coordinatized  in  the  local 
basis  is  given  by 


*  • 

V 

-  - 

♦* 

(A 

-  Ay) 

(AKglux  +  ra12wy 

+  m13uz) 

ex 

♦y 

=  - 

(A 

■  “A) 

+ 

(m21ux  +AKg2uy 

+  m23uz) 

6y 

*z 

m  « 

(Ay 

-  w  \|r  ^ 

y  x) 

(m31wx  +  m32wy 

+  AKg3wz) 

6z 

Coordinatizing  Eq.  (2.13)  in  the  local  basis  gives 


.  (2J 


10  0 


[0  0  1] 


(2.23) 


b  = 
L 


/ 

* 


(2.24) 


and 


vj/  x  f  - 

L  L 


f 

z 


(2.25) 


Carrying  out  the  indicated  operations  and  substituting  into  Eq. 
(2.13)  yields  the  following  position  error  equations  coordinatized  in 
the  L- basis: 


P>X 

(<y  z  - 

~(h)yhz  - 

«=  -  2 

|W  f*x  -  (0  f>z) 
\z  x  ; 

- 

f>x  -  w  p>z  | 

V  z  X  ) 

F.Z 

L'  f.y  -  u»  P-.x) 

X  x  y  / 

(u  fty  -  d)  f*x\ 

l\  *  y  / J 

i 


T'V  i u  ijHjypp 


'  Sx 

(ak  f 

V  fl  X 

+  m12fy 

+  m13fz) 

sy 

+ 

(”21£x 

+  AKf2fy 

+  m23fz) 

-  2$z 

(”31£x 

+  m32fy 

+  *Kf3  fz) 

b  ‘ 

$  f  -  t  f 

X 

y  z  z  y 

b 

ff  -  \|r  f 

y 

Z  X  X  z 

b 

f  f  -  \|r  f 

z 

x  y  y  x 

(2.26) 


2.4  THE  SCALAR  FORM  OF  THE  TORQUING  EQUATIONS 

— > 

2.4.1  Coordinatization  of  Vector  m  in  the  Tangent  Plane 
Mechanization 

The  IMU  equations  of  error  angle  between  the  platform  and 
computer,  Eq.  (2.19),  and  the  position  error,  Eq.  (2.26)  can  be  made 
more  explicit  through  one  more  expansion  on  the  vector  u.  Recall,  that 
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tj  is  the  angular  rate  at  which  the  platform  is  torqued  or  rotated,  rel¬ 
ative  to  inertial  space,  about  its  nominal  X,  Y,  and  Z  axes.  Regarding 

t 

the  local  basis,  L,  as  the  true  or  computer  basis,  C,  (the  basis  in 
which  the  computations  are  performed  to  update  velocity,  position  and 
angular  velocity  terms)  the  vector  u  can  be  coordinatized  in  any  of  the 
bases  previously  mentioned.  If  the  tangent  plane  mechanization  is 
chosen,  the  platform  angular  rates  are  constant  rather  than  time 
varying  as  in  all  of  the  others.  Thus,  the  platform  is  held  fixed 
relative  to  the  fixed  point  on  the  earth,  regardless  of  the  vehicle 
position.  Fig.  2.5  displays  the  geometry  and  shows  the  tangent  plane 


FIG.  2.5.  TANGENT  PLANE  MECHANIZATION  GEOMETRY 


eminating  from  the  fixed  point  passing  through  the  launch  site.  By 

C*I 

inspection,  the  components  of  the  gyro  torquing  signals  to  in  the 

v 

computer  basis  are  given  by 

0 

ft  cos  \  .  (2.27) 

o 

ft  sin  \ 

o. 


The  following  analysis  is  performed  as  a  check.  The  coordinatiza- 
tion  begins  with 


C-I  C-E  E-I 

L)  =  (i)  +  U' 

c  c  c 


(2.28) 


✓ 


22. 


To  keep  the  requirement  that  only  constant  gyro  torques  are 


mechanized , 


C-E 

03  =  0 

C 


(2.29) 


— >E-I 

Now  a)  is  best  coordinatized  in  the  earth’s  fixed  basis  in  which  it 
is  known  and  nominally  constant,  i.e., 


(2.30) 


Thus,  to  express  Eq.  (2.28)  in  the  common  basis  given,  a  transformation 
is  required  on  Eq.  (2.30), 


E-I  „  E-I 
0  *  C/E  E 


(2.31) 


where  TC^E  is  defined  as  the  direction  cosine  matrix  representing  the 
coordinate  transformation  from  the  earth's  basis  (E)  to  the  computer's 
basis  (C) . 

The  summary  of  the  transformation  is  shown  in  Table  2.1. 

Table  2.1 

TRANSFORMATION  SUMMARY 


Angle  Axis 

Transformation  of  Rotation  of  Rotation  Basis  Name 


Earth 


Computer 


This  results  in  the  following: 


C/E 


because 


1 

0 

0 

cos  Ao 

0 

-sin  A0‘ 

0 

cos  \ 

o 

-sin  k 

0 

0 

1 

0 

0 

sin  \ 

0 

cos  ^ 

0 

sin  A 0 

0 

cos  Aq 

r 

n 

C-I 


u 

C 


0) . 


w.. 


w_ 


,  (2.32) 


(2.33) 


then 


w 

'o 

X 

(ll 

n  cos  \ 

y 

o 

w 

Q  sin  \ 

z 

o 

(tangent  plane  mechanization).  (2.34) 


In  this  case,  the  base  point  or  fixed  point  latitude,  \  ,  is  used 
throughout  the  mission  and  the  torquing  rates  applied  to  the  gyros 
are  shown  in  Eq.  (2.34). 


2.4.2  Coordinatization  of  Vector  u>  in  the  Space  Fixed  Tangent 
Plane  Mechanization 

For  completeness,  the  following  discussion  concerns 
another  mechanization  scheme  that  is  simple  enough  to  be  competitive 
with  the  tangent  plane  mechanization.  For  lack  of  a  more  widely 
accepted  terminology,  it  is  called  the  space-fixed  tangent  plane 
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mechanization.  It  was,  in  fact,  used  on  some  versions  of  the  PERSHING 
guidance  system.  The  local  level  is  established  via  IMU  mounted 
pendulums  or  precision  accelerometers  and  there  is  no  torquing  of  the 
azimuth  gyro.  This  mechanization  is  identical  to  the  tangent  plane 
scheme  described  in  detail  earlier.  However,  in  the  space- fixed 
tangent  plane  mechanization,  the  computation  of  earth’s  rate  is 
terminated  inmediately  before  launch  so  that  the  horizontal  and  verti¬ 
cal  components  of  earth  rate  torquing  to  the  level  gyros  are  also 
zero.  That  is,  in  this  mechanization 


'co  * 

■o* 

X 

10 

0 

y 

CO 

0 

z_ 

(space  fixed  tangent  plane  mechanization) . 


(2.35) 


The  obvious  advantage  to  mechanizing  a  scheme  that  does  not  torque 
the  gyros  is  more  than  a  simplification  in  the  on-board  computer.  The 
entire  inertial  instrumentation  package  is  made  at  least  an  order  of 
magnitude  less  precise  in  terms  of  manufacturing  tolerances.  Torquer 
linearity,  precision  pickoffs,  voltage  and  current  supplies,  and  pulse 
and  analog  circuits  benefit  from  this  consideration. 

The  burden  of  the  simplified  on-board  hardware,  in  the  case  of 
PERSHING,  is  placed  on  the  ground  based  level  and  alignment  hardware. 
Though  the  earth's  rate  components  are  not  calculated  nor  used  to 
torque  the  two  level  gyros,  a  set  of  firing  tables  are  required  to 
offset  the  missile's  trajectory  to  the  primary  target  to  compensate  for 
the  Coriolis  acceleration.  (The  laws  of  nature  still  remain  fixed 
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regardless  of  the  mechanization  chosen.)  Consequently,  it  is  a  matter 
of  choice,  based  on  the  preceding  alternatives,  as  to  which  of  the  two 
simplest  mechanizations  one  is  willing  to  instrument.  For  purposes  of 
this  study,  the  more  self-contained  version,  the  true  tangent  plane 
mechanization  will  be  used.  In  the  simulation  results  which  are  pre¬ 
sented  later,  there  is  practically  no  difference  in  using  this  choice 
other  than  replacing  by  zero  the  constants  in  the  system's  F  matrix. 

2.4.3  Coordinatization  of  Vector  (j  in  the  Free  Azimuth 

Mechanization 

The  utility  of  using  a  northern  reference,  per  se,  is 
susceptible  to  questioning  in  the  case  of  a  missile  terminal  guidance 
scheme.  That  it  is  of  fundamental  importance  in  terrestial  navigation 
or  terrestrial  dead  reckoning  is,  however,  gospel.  This  derivation  of 
a  mechanization  scheme  and  the  next  two  that  follow  are  north 
referenced  because  they  are  considered  in  the  framework  of  a  navigation 
problem.  That  is,  the  missile  terminal  guidance  scheme  is  based  upon 
maps  that  have  been  generated  in  the  context  of  local  earth  coordinates 
which  include  a  precise  reference  to  north.  Anticipating  the  results 
however,  makes  the  argument  somewhat  academic  because  the  additional 
state  variables  required  to  define  a  north  reference  preclude  these 
mechanizations  on  economic  grounds.  These  arguments  are  discussed 
more  fully  in  later  chapters. 

This  mechanization  eliminates  the  torquing  error  associated  with 
the  Z-gyro,  i.e.,  the  gyro  with  its  input  axis  vertical,  because  it 
does  not  provide  a  torquing  command  about  the  vertical  axis.  Instead, 
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t  a  Indicated  north  direction  is  computed  by  calculating  the  angle 
between  the  north  given  in  a  level  plane  and  the  level  plat  ton.  ares 

a”8le’  Sh°m  in  Pls-  2-6  *»•  angle  between  the  horiaontal 

platform  Y  axis  and  the  true  north. 


FIG.  2.6.  FREE  AZIMUTH  MECHANIZATION  GEOMETRY 


As  in  the  previous  case,  the  local  basis  (L)  is  considered 

the  computer  or  true  basis.  Equation  (2.18)  is  repeated  for 
convenience , 


to  be 


C-E  ,  E-I 
w  +  w 

C  C 


a  transformation  on  the  earth's  rate  is  valid 


Again  the  argument  for 


Unlike  the  previous  case,  where  it  was  considered  zero, 


->C-E 

0) 


+  \  Z, 


+  A  Ye 


(2.39) 


Here,  ZT  is  the  unit  vector  about  the  Z  axis  of  the  platform,  and  2L, 

Jj  “ 

and  Y„  are  the  unit  vectors,  respectively,  about  which  small  rotations 
£ 

of  angles  \  and  a  are  made  in  the  intermediate  Euler  sequence.  To 
coordinatize  all  the  vectors  into  the  computer  basis,  the  a  rotation 
needs  no  transformation,  the  \  term  is  transformed  through  the  angle  a 
to  determine  its  components  in  the  computer  basis ,  and  the  A  term  is 
transformed  into  the  computer  axes  through  the  angles  a  and  The 
coordinatization  is  given  by 


0 

1 

rL 

0 

** 

# 

0 

+  tc/ze 

0 

+  tc/e 

A 

• 

a 

0 

0 

(2.40) 


The  minus  sign  on  \  indicates  that  the  platform  is  maintained  locally 
level  by  torquing  the  X-gyro  to  produce  the  precession  rate  [15] . 
The  transformation  T is  summarized  in  Table  2.3. 

Table  2.3 


TRANSFORMATION  SUMMARY 


Transformation 

Angle 

of  Rotation 

Axis 

of  Rotation 

Basis  Name 

T** 

/ 

Larth 

C/E 

— )  \ 

\ 

"XE  J 

a 

\  \ 

L 

/ 

Computer 

\ 
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The  result  Is 


** 

lC/E 


•  «■ 

cos  a  sin  a  0 

‘l  0  0 

ss 

-sin  a  cos  a  0 

0  cos  \  -sin  \ 

0  0  1 

0  .  sin  X.  cos  X. 

(2.41) 


T_,„  is  a  simple  transformation  resulting  from  a  small  rotation  of 
C/ZE 

angle  a  about  the  axis.  Thus, 

cos  a  sin  a  0 


C/Zt 


•sin  a  cos  a  0 
0  0  1 


(2.42) 


Substituting  Eqs.  (2.41)  and  (2.42)  into  Eq.  (2.40)  yields 


C-E 


O) 

C 


-X.  cos  a  +  A  sin  Ci  cos'  X. 
\  sin  a  +  X  cos  a  cos  X. 
a  +  a  sin  X. 


(2.43) 


Substituting  Eqs.  (2.38)  and  (2.43)  into  Eq.  (2.28)  gives  the  following 
coordinatization  result: 


-  « 

0) 

X 

"  ♦  • 

(n  +  A)  sin  a  cos  X.  -  X.  cos  a 

wy 

- 

(fl  +  A)  cos  a  cos  X.  +  \  sin  a 

(a) 

z 

(n  +  A)  sin  X.  +  a 

m 

(2.44) 


As  previously  mentioned,  one  of  the  main  advantages  of  the  free 
azimuth  mechanization  results  in  not  having  to  torque  the  azimuth  or 
Z-gyro,  i.e.,  it  is  free  to  rotate.  (Another  similar  mechanization, 
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called  the  wander  azimuth  mechanization,  results  when  the  Z  component 
_ >C — E 

of  co  is  constrained  to  equal  the  Z  component  of  ft  .)  Thus,  to 

z 


is  made  zero  by  forcing 


a  =  - (ft  +  a)  sin  a 


(2.45) 


which  results  in 


CJ 

X 

(ft  +  A)  sin  ft  cos  \  -  \  cos  a 

CJ 

y 

s 

(ft  +  A)  cos  a  cos  \  +  \  sin  a 

CJ 

0 

z 

„ 

(free  azimuth 
mechanization) 

(2.46) 


2.4.4  Coordlnatization  of  Vector  to  in  the  Local  Level. 

North-East  Mechanization 

Another  coordinate  mechanization  widely  used  for  naviga¬ 
tion  which  has  potential  for  this  application  is  the  local  level, 
north-east  system.  The  usefulness  of  this  system  is  important  when  the 
system's  outputs  are  desired  corresponding  to  map  data  or  when  an 
explicit  vertical  is  desired  to  drive  auxiliary  equipment.  This  basis 
is  also  observable  in  Fig.  2.7,  but  with  the  angle,  a,  now  kept 
constant  at  the  value  of  zero. 

The  platform  torquing  rate  is  again  repeated 


Now 


C-I  C-E  ,  E-I 
w  =  w  +  gj 

c  c  c 


E-I  ***  E-I 

w  =  Tr/_  cj 

C  C/E  E 


(2.47) 
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where  is  of  the  same  form  as  the  transformation  Tc  ,£  of  Eq.  (2.32) 


i.e., 


1  0 


cos  A  0  sin  A 


Tc^e  =  0  cos  \  -sin  \  0 


1  0 


0  sin  \  cos  \  sin  A  0  cos  A 


Again,  the  earth  rate  components  are 


(2.48) 


E-I 

u  =  fl 


(2.49) 


so  that 


E-I 

w  =  n  cos  \ 


0  sin  \ 


(2.49) 


The  term  w  is  similar  to  its  counterpart  in  the  free  azimuth 
C 

system;  only  here  it  is  less  complex,  i.e., 


„C-E  -  T. 


(2.50) 


cos  \  sin>..  a  +  0 


(2.51) 


•sin  X.  cos  \  I  1 0 


-32- 


Adding  Eqs.  (2.49)  and  (2.51)  gives  tne  final  result 


u> 

'-k 

X 

wy 

= 

(fl  +  A)  cos  \ 

(local  level  north-east  mechanization)  .  (2.52) 

wz 

(a  +  A)  Sin  \ 

Notice  that  it  is  a  special  case  of  the  free  azimuth  mechanization 
given  by  Eq.  (2.46)  but  with  a  set  equal  to  zero  in  this  case. 

Sufficient  analysis  has  been  performed  to  display  the  coordi- 
natization  of  the  IMU's  rate  of  rotation  or  torque  rate,  u|  in  four 
mechanizations.  The  rationale  for  the  specific  choice  best  suited  to 
this  application  is  discussed  in  more  detail  in  Chapter  111. 


CHAPTER  III.  MATHEMATICAL  MODELING  OF  THE  SYSTEM 


3.1  IDENTIFICATION  OF  THE  INERTIAL  SYSTEM  MECHANIZATION 

The  first  step  in  applying  the  Kalman-Bucy  theory  is  to  identify 
the  system  on  which  the  filter  is  to  be  based.  In  the  case  of 
improving  the  guidance  of  a  reentry  vehicle,  it  may  seem  that  the  most 
direct  choice  would  be  a  system  that  estimated  the  desired  parameters 
of  the  vehicle.  The  filter  would  then  be  based  on  the  equations  that 
describe  the  motions  of  the  vehicle  itself.  This  approach  is  function¬ 
ally  visualized  in  Fig.  3.1.  Though  appealing  for  an  orbiting  space¬ 
craft  guidance  problem  where  the  position  and  velocity  could  be  pre¬ 
dicted  for  any  future  time  with  accuracy,  it  is  not  easily  implemented 
for  a  rapidly  varying  dynamic  system. 


FIG.  3.1.  FUNCTIONAL  DIAGRAM  OF  A  DIRECT  FILTER 


Instead,  an  indirect  filter  is  implemented  based  on  kinematic  con¬ 
siderations.  It  is  worthwhile  to  diverge  at  this  point  for  a  brief 
discussion  relating  the  notions  of  dynamics,  kinematics,  and  measure¬ 
ments  in  the  context  of  this  problem. 

Dynamics  is  expressed  by  some  as  the  study  of  the  motion  of  a 
particle  (system  of  particles)  from  the  knowledge  of  the  external 
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forces  acting  on  it.  Kinematics  is  sometimes  expressed  as  the  study 
of  the  motion  of  a  particle  (system  of  particles)  disregarding  the 
forces  associated  with  the  motion.  Simply,  it  is  the  study  of  the 
geometry  of  the  motion  relating  time,  displacement,  velocity,  accelera¬ 
tion,  etc.,  both  translational  and  rotational. 

Confusion  may  arise  because  control  engineers  use  the  terminology 
"dynamic  systems"  to  describe  a  plant  by  equations  which  vary  as  a 
function  of  time.  This  is  often  done  to  emphasize  that  the  system 
under  discussion  is  not  static.  An  analytical  dynamicist,  however, 
uses  the  terminology  "dynamic  equations"  to  define  the  set  of 
equations  describing  motion  of  a  particle  acted  on  by  forces  as  previously 
summarized.  Thus  to  a  dynamicist,  the  Euler  equations  of  rigid  body 
mechanics,  for  example,  are  dynamic  expressions  while  the  equations 
relating  angular  velocity  among  reference  bases  are  kinematic 
expressions . 

Measurements  can  be  thought  of  as  one  of  the  three  types  of  inputs 
for  a  Kalman-Bucy  filter.  In  a  navigation  or  guidance  problem,  the 
measurement  may  be  a  doppler  radar  measuring  velocity  components,  it 
may  be  a  Loran  receiver  measuring  time  differences  in  radio  wave  propa¬ 
gation,  or  it  may  be  a  radar  area  correlator  as  in  this  problem.  The 
second  input  is  the  driving  noise  associated  with  gyro  drift,  SFM  bias, 
etc.  The  third  input  is  the  main  forcing  variable  of  the  differential 
equation  which  may  be  a  torque,  a  force,  or  othdr  driving  function. 
Thus,  in  the  expression  for  a  plant  given  by 

x(t)  =  F (t)  x(t)  +  G(t)  u(t)  +  w(t)  , 
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with  a  measurement  given  by 


Z(t)  =  H(t)  x(t)  +  v(t) 

The  variable  u(t)  is  the  forcing  variable,  the  variable  w(t)  is  the 
driving  noise,  Z(t)  is  the  measurement  of  x(t)  with  a  corruption  of 
additive  noise  v(t). 

In  a  direct  filter  formulate^  for  a  vehicle  carrying  an  IMU,  the 
dynamic  system  (control's  sense)  on  which  the  filter  is  based  is  the 
system  of  equations  that  describe  the  motions  of  the  vehicle  itself 
(dynamicists'  sense).  The  filter  would  use  all  measurements,  including 
those  of  the  IMU,  to  produce  estimates  of  the  position  and  velocity  of 
the  vehicle  directly.  The  dynamic  equations  describing  the  system 
requires  a  statistical  dynamic  model  for  the  vehicle  to  be  indue in 
the  state  space  formulation  [16]  .  However,  the  model  used  to  describe 
these  random  motions  is  difficult  to  obtain  for  a  vehicle  rapidly 
varying  in  velocity  and  position  as  a  function  of  time.  In  fact, 
measurements  of  vehicle  acceleration  and  angular  velocity  are  much 
better  data  to  process  than  to  model  the  disturbances  or  forces  which 
cause  them. 

The  indirect  filter  is  a  completely  different  way  of  formulating 
the  navigation  problem  which  avoids  most  of  the  practical  problems  of 
the  previous  method  if  in  addition  to  the  inertial  navigation  system 
there  is  included  some  other  source  of  navigation  data.  Instead  of 
estimating  the  state  of  the  vehicle  directly,  the  filter  is  used  to 
estimate  the  error  state  of  an  inertial  navigation  system.  The 
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inertial  system  follows  the  high  frequency  motions  of  the  vehicle  very 
accurately  but  has  low-frequency  errors  which  grow  with  time.  The 
dynamic  system  on  which  the  filter  is  based  is  the  set  of  error 
equations  for  the  inertial  system  which  are  relatively  well  Known, 
well  behaved,  low  frequency,  and  essentially  linear.  The  sample  period 
can  range  from  several  seconds  up  to  a  minute  without  greatly 
influencing  the  effectiveness  of  the  filter.  For  these  reasons,  this 
method  is  used  for  virtually  every  practical  terrestrial  referenced 
IMU  Kalman  filter  mechanization.  In  the  particular  case  of  navigating 
the  reentry  body,  the  time  of  flight  under  this  condition  is  so  short 
that  the  indirect  scheme  can  be  functionally  implemented,  as  shown  in 
Fig.  3.2. 


FIG.  3.2.  FUNCTIONAL  DIAGRAM  OF  AN  INDIRECT  FILTER 


Note  that  the  outputs  from  the  inertial  system  are  not  the 
measurements  in  the  Kalman- Bucy  theory.  Rather,  they  are  the  forcing 


function  or  driving  input  in  the  dynamical  equations  describing  the 
system.  The  radar  area  correlation  system  and  the  radar  altimeter  are 
the  measurement  kinematics  as  far  as  this  estimation  problem  is 
concerned . 

The  error  equations  in  inertial  systems  position  and  velocity, 
given  as  Eqs.  (2.19)  and  (2.26)  are  logical  choices  for  the  inertial 
system  states.  Rewriting  them  in  a  manner  more  amenable  to  manipula¬ 
tion  as  state  variables  follows.  From  Eq.  (2.26), 
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From  Eq.  (2.19), 


*x  =  Vz  ‘  Vy  +  AKglwx  +  m12wy  +  013wz  +  ex 

• 

\|r  =  -\[r  u>  +  \|f  w  +  m.-w  +  AK  0tj  +  moo0)  +  e 
Yy  x  z  z  x  12  x  g2  y  23  z  y 

and 

tjf  =  t  i>)  -  +  ra0,(j  +  m0ouj  +  AK  -w  +  e 

Yz  x  y  y  x  31  x  32  y  g3  z  z 


(3.4) 

(3.5) 

(3.6) 


3.1.1  State  Vector  for  the  Tangent  Plane  Mechanization 

From  Eqs.  (3.1)  through  (3.6),  part  of  the  state  vector  is 

chosen  to  be 

XT  =  [sx,  By,  52,  5Vx,  6Vy,  &Vz,  x|ry,  ^  J  ,  (3.7) 

where 

6x  =  5V^ A  error  in  IMU  x-velocity 

5y  =  BVyAerror  in  IMU  y-velocity 

Sz  =  5VzAerror  in  IMU  z-velocity, 

and  other  variables  are  as  previously  defined. 

The  gyro  torquing  rates  w  ,  w  ,  and  w  implicit  in  Eq.  (3.7)  were 

x  y  z 

shown  to  differ  according  to  the  mechanization  scheme.  The  tangent 
plane  mechanization  torquing  rates  are  constants  given  by 

w  =  0 
x 

COy  =  n  cos  \q 

(j  '=  (2  sin  \  .  (3.8) 

z  o 

Because  there  are  no  additional  states  required  to  define  the  torquing 
rates  for  this  mechanization,  the  state  vector  would  remain  as  given 
by  Eq.  (3.7). 


39- 


Ji  WHfWBBPgiPBWro^  LJ  .1,111.11  Km^WTMW!"  "W 


3.1.2  State  Vector  for  the  Space-Fixed  Tangent  Plane 

Mechanization 

The  torquing  rates  in  the  space-fixed  tangent  plane  mecha¬ 
nization  are  equally  as  simple  and  also  constant.  To  be  exact,  the 
constant  is  zero.  However  for  the  reasons  previously  described,  the 
space- fixed  tangent  plane  mechanization  is  rejected  in  favor  of  the 
true  tangent  plane  mechanization. 

3.1.3  State  Vector  for  the  Free  Azimuth  Mechanization 

The  free  azimuth  torquing  rates  as  described  in  Eq.  (2.46) 
are  complicated  by  the  additional  explicit  dependence  on  latitude  and 
longitude  rates,  \  and  /,  respectively.  Also  the  wander  angle,  a,  is 
seen  as  an  independent  variable.  For  consistency  then,  the  error  in 
these  three  variables  must  be  derived  and  included  in  the  state  vector. 
The  perturbation  in  these  variables  may  be  rationalized  as  follows. 
Until  now  the  assumption  was  used  that  the  local  basis  (L)  was  equal 
to  the  computer  or  true  basis  (C).  Generally,  the  possibility  exists 
that  the  computer  is  in  error  by  some  small  amount  in  its  calculation 
of  the  actual  position  as  given  in  the  local  basis.  The  development 
which  follows  depicts  the  effect  by  way  of  perturbations  on  the  ideal 
equations . 

Refer  to  Fig.  2.6  to  visualize  the  ideal,  errorless  rates  given  by 
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Also , 


and 


r 


cos  \ 


(3.11) 


A  = 


V  cos  a  -  V  sin  a 

_ I _ 

^Rg  +  h\  cos  \ 


(3.12) 


where : 

Vjj  A  vector  representing  velocity  in  northerly  direction 

Vg  A  vector  representing  velocity  easterly  direction 

h  A  altitude  above  the  reference  ellipsoid 

Rg  A  radius  of  curvature  of  reference  ellipsoid  in  . 

“  easterly  direction 

R-j  A  radius  of  curvature  of  reference  ellipsoid  in 
=  northerly  direction 

Vx  A  component  of  velocity  along  platform's  x-axis 
Vy  A  component  of  velocity  along  platform's  y-axis 
A  perturbation  on  the  latitude  and  longitude  rate  equations  yields 
the  differential  equations  for  the  latitude  and  longitude  errors. 

Thus,  from  Eq.  (3.10) 


•J£(X  +  5\)  = 


(Vx  +  oVx)  sin  (a  +  &a)  +  (Vy  +  &Vy)  cos  (a  +  8a) 


*N 


+  h 


(3.13) 


Noting  that 

cos  8a  »  1 

sin  sa  «  sa 


(3.14) 

(3.15) 
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and  expanding  the  sine  and  cosine  functions  by  their  trigonometric 
identity  results  in 


5\  = 


V  cos  aba.  +  &V  sin  a  -  V  sin  a6  a  4-  &V  cos  a 
x  x  2. _ _ - _ £ _ 


*N 


+  h 


(3.16) 


Similarly,  from  Eq.  (3.12) 


■jjrOV  +  &A)  = 


(vx  +  &Vx)  cos  (a  +  s a)  -  (vy  +  &Vy)  sin  (a  +  5a) 


+  h^  cos  (X.  +  5X.) 


(3.17) 


Ti:n  result  is 


.  -V  sin  as  a  +  &V  cos  a  -  V  cos  a&  a  -  &V  sin  a 
5A  -  — - - • - - - * - *- 


(RE+h) 


sin  X.&X. 


(3.18) 

The  variation  in  the  wander  angle,  a,  is  obtained  from  Eq.  (2.45) 


viz, 


a  =  -  (ft  +  A)  sin  X. 


(3.19) 


Again,  to  first  order 

5a  =  (ft  +  A)  cos  X.&X.  -  sin  X.&A  .  (3.20) 


Substituting  Eq.  (3.12)  into  Eq.  (3.20)  yields 


5Q  51 


(“ 


5x  cos  a  -  6y  sin 
(Re  +  h^  cos  X. 


cos  X.6X.  -  sin  X.5A 


(3.21) 


These  three  error  differential  equations,  required  to  extend  the 
state  vector  when  they  appeared  in  the  torquing  equations,  have  in  turn 
generated  the  requirement  for  the  inclusion  of  the  true  or  idealized 
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velocity  components  Vx  and  Vy.  Both  are  obtained  from  Eq.  (2.8a)  but 
must  be  coordinatized  in  the  free  azimuth  basis..  That  is,  nominally 


V  = 
L 


(3.22) 


and  from  Eq.  (2.8b) 


L 


(3.23) 


For  this  mechanization,  the  state  vector  would  be 


XT  =  Ts 


[»*.  Sy»  5Z,  5VX,  5Vy,  &Vz,  *x,  T|ry,  t|fg,  8X,  BA,  BCr]  (3.24) 


The  increased  number  of  states  required  to  mechanize  this  scheme  is 
evidence  by  comparing  this  state  vector  with  the  state  vector  described 
by  Eq.  (3.7). 

3.1.4  State  Vector  for  the  Local  Level  North-East  Mechanization 
The  local  level  north-east  mechanization  would  have  gyro 
torquing  rates  given  by  Eq .  (2.52), 

(ox  =  -X 

wy  =  (n  +  a)  cos  x 
u  a  <n  +  a)  sin  x 

z 

Again,  there  is  a  need  for  the  error  differential  equation  describing 
X  and  A«  However,  without  the  need  for  a,  the  state  vector  is  simpli¬ 
fied  by  one  state  over  the  previous  mechanization  as 
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(3.25) 


XT  =  |sx,  By,  8z,  6Vx,  5Vy,  6Vz,  *x,  *y,  t2»  6*.,  BA  ] 

3.1.5  State  Vector  Augmentation 

In  addition,  there  is  a  possibility  that  gyro  and  accel- 
rometer  errors  are  correlated  requiring  six  more  augmented  states  to 
be  included  by  addition  to  each  model  mechanized,  i.e., 


Therefore,  a  fully  mechanized  inertial  system,  with  6  augmented 
gyro  and  accelerometer  states  a  possibility,  can  be  modeled  with  as  few 
as  15  states  in  the  tangent  plane  or  as  many  as  18  states  in  the  free 
azimuth.  The  local  level  north-east  mechanization  is  a  compromise 
requiring  17  states.  It  is  therefore  reasonable  Lo  choose  a  mechaniza¬ 
tion  based  on  the  constraint  that  the  airborne  computer  will  have  only 
limited  capability  to  accommodate  the  filter  implementation.  Because 
the  optimal  filter  requires  computation  of  the  Riccati  error  covari¬ 
ance  equation,  it  alone  requires  n(n  +  l)/2  equations  based  on  n  number 
of  states.  The  minimum  number  of  equations  that  roust  be  solved, 
including  the  number  of  states  required  to  model  only  the  inertial 
system,  are  seen  in  Table  3.1. 


Table  3.1 

COMPARISON  OF  MECHANIZATION  COMPLEXITY 


Mechanization 

No.  of  Equations 

Tangent  Plan 

135 

Space-Fixed  Tangent  Plane 

135 

Local  Level  North-East 

153 

Free  Azimuth 

171 
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To  obtain  a  torque- free  azimuth  gyro  in  the  free  azimuth  mechanization, 
a  27  percent  increase  in  computer  capability  compared  to  the  tangent 
plane  is  required.  The  tangent  plane  mechanization  requires  12  percent 
less  computer  capability  than  does  the  local  level  north-east.  For 
this  initial  analysis,  the  tangent  plane  mechanization  is  chosen  and 
the  first  objective  of  this  study  is  met. 

3.2  CHOICE  OF  THE  MODEL'S  TRAJECTORY 

Additional  considerations  simplify  the  state  space  description  of 
the  total  system  even  more.  For  purposes  of  this  report,  the  PERSHING 
trajectory  can  be  represented  as  a  parabolic  arc  with  the  baseline 
reaching  a  maximum  of  400  nautical  miles  and  a  maximum  height  of  120 
miles  from  the  earth’s  surface.  Results  of  previous  studies  have 
dictated  that  the  terminal  guidance  phase  be  initiated  at  an  altitude 
of  30,000  feet  above  the  earth’s  surface.  The  trajectory  is  shown  in 
Fig.  3.3  in  the  IMU  X-Z  plane.  The  velocity  of  the  reentry  body  at  the 
30, 000- foot  level  is  approximately  3000  feet  per  second  in  the  negative 
Z  direction  (down).  Also,  the  velocity  is  almost  constant  from  this 
altitude  to  impact.  Actually,  it  will  vary  according  to  the  vehicle's 
ballistic  drag  coefficient,  air  density,  exact  altitude  of  the  target, 
specific  reentry  angle  of  attack,  etc.  However,  the  model  is  simpli¬ 
fied  by  not  including  the  vehicle's  aerodynamic  characteristics. 
Previous  flights  have  shown  the  time  to  impact  from  this  altitude 
varies  from  approximately  7  to  12  seconds.  For  convenience,  a  flight 
time  of  10  seconds  is  used  in  the  model's  simulation. 

An  additional  comment  is  necessary  concerning  the  simulation  of 
the  trajectory.  Although  a  constant  velocity  of  -3000  feet  per  second 
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FIG.  3.3.  TYPICAL  PERSHING  FLIGHT  PROFILE 

in  the  Z  direction  is  reasonable,  there  could  be  an  additional  Y  or  X 
component.  To  eliminate  one  state  the  trajectory  is  constrained  to  the 
X-Z  plane,  with  no  loss  in  generality,  because  the  Y  component  could  be 
eliminated  choice  of  axes.  Thus,  the  f  and  f  specific  force  terms  in 
the  IMU  mechanizations  are  initialized  at  zero  because  their  corre¬ 
sponding  velocities  are  constant.  As  will  been  seen,  however,  this  is 
only  of  academic  interest  because  additional  simplifications  eliminate 
even  those  terms.  The  result  is  a  free  trajectory,  i.e.,  there  is  no 
state  modeled  representing  actual  vehicle  position,  velocity,  or  accel¬ 
eration  as  was  discussed  previously.  The  system  states  are  IMU  errors 
in  these  domains  and  are  driven  by  IMU  error  inputs.  When  the  specific 
force  terms  are  neglected  there  is  no  physically  meaningful  trajectory 
generated,  not  to  imply  that  it  cannot  be  done.  However,  in  this 
model,  there'  is  ample  justification  to  neglect  them. 


3.3  FORMULATION  OF  THE  IMU  ERROR  MODEL 

The  terminal  guidance  phase  is  only  10-seconds  long  for  the  appli¬ 
cation  in  which  the  filter  will  be  implemented.  The  effect  of  correc¬ 
ting  the  gyro  and  accelerometer  errors,  which  propagate  with  an  84- 
minute  period,  is  negligible  over  this  short  time  span.  In  addition, 
their  effect  on  system  error  buildup  is  known  from  extensive  flight 
test  and  analytic  data  (Martin-Marietta  Report  [17]). 

Instead  of  modeling  error  sources  which  propagate  at  negligibly 
low  frequency,  i.e.,  the  platform  tilts,  ty,  and  the  gyro  and  accelerom- 
eter  scale  factor  errors,  AKg,  and  AKf,  respectively,  their  random 
errors,  ~c,  and  the  accelerometer  bias,  b,  their  effects  on  the  system 
are  included  as  driving  noise  in  the  IMU  error  equations.  Because  gyro 
or  accelerometer  errors  are  not  modeled,  the  six  states  can  be  elimina¬ 
ted,  as  given  in  Eq.  (3.26).  Three  more  states  are  eliminated  by  not 

modeling  the  tilt  equations.  It  should  be  noted  that  to  assume  \jr  is 

— *  A 

zero  is  not  exactly  true.  The  argument  is  that  \Jr  and  are  zero 
because  the  effect  of  the  tilts  on  IMU  performance  at  initiation  of  the 
terminal  guidance  phase  can  be  included  as  a  forcing  term  in  the  IMU 

■4 

error  equations;  i.e.,  the  total  error  accrued  during  the  flight  is 
mechanized  in  the  filter  as  an  initial  condition.  Its  buildup  and 
additional  contribution  to  the  IMU  error  is  considered  negligible  in 
this  application  during  the  10-second  period.  Longer  flights,  even  in 
the  order  of  minutes  in  other  applications  for  example,  and  in  cases 
where  the  data  are  not  available  to  permit  this  alternative  treatment 
would  invalidate  the  simplification. 
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With  \|r  and  \|r  zero,  Eqs.  (3.1)  through  (3.6)  simplify  to  the  degree 


of  excluding  the  specific  force  terms  f  ,  f  ,  and  f  ,  as  was  alluded 

x  y  z 

to  previously.  With  no  other  means  to  propagate  a  physical  trajectory, 
the  IMU  error  equations  are  seen  in  a  free  trajectory  for  10  seconds  in 
which  the  accumulated  system  errors  to  that  point  drive  the  system  as 
initial  conditions. 

The  ftV  state  is  excluded  because  the  model  is  constrained  to  the 

y 

X-Z  plane  for  this  analysis.  This  is  not  to  imply  that  because  6y  is 
initialized  equal  to  zero,  SV^  can  be  neglected.  The  converse  is 
true.  There  is  every  reason  to  believe  that  the  error  state  pairs  6x 
and  by  as  well  as  ?,VX  and  SV^  will  be  nearly  equal  for  this  applica¬ 
tion.  However,  to  save  computer  memory  and  operating  time,  only  one  of 
the  velocities  will  be  estimated,  in  this  case  5Vx<  The  results, 
shown  later,  should  be  interpreted  to  mean  that  the  estimates  for  bVx 
are  equally  likely  to  be  representative  of  bV^. 

With  so  much  emphasis  on  eliminating  state  variables,  it  is  almost 
incongruous  to  justify  keeping  two  states  that  are  most  often  elimina¬ 
ted.  The  bz  and  bV  states  are,  in  every  reference  source,  shown  to  be 
z 

in  a  divergent  or  unstable  mechanization  and  are  thus  not  instrumented. 
In  most  terrestrial  navigators,  the  Z  accelerometer  is  not  even  physi¬ 
cally  mounted  on  the  IMU.  It  was  observed  by  Kayton  [18]  that  the 
error  in  the  altitude  channel  grows  exponentially  and  doubles  in  the 
amplitude  in  approximately  28  minutes.  However,  the  total  PERSHING 
flight  is  less  than  7  minutes  and  the  terminal  guidance  phase  is  almost 
two  orders  of  magnitude  smaller  than  that.  Consequently,  the  Z-channel 


instability  will  not  cause  a  catastrophically  larger  error  in  this 
case,  given  reasonable  initial  conditions.  Because  the  Z-channel 
information  is  very  desirable  for  on-board  functions  such  as  arming, 
safing,  and  fuzing,  and  because  it  could  be  useful  in  scheduling 
imagery  and  gains  in  one  configuration  of  a  suboptimal  filter,  it  is 


included  in  the  IMU  error  mechanization  equations. 

As  a  consequence  of  these  decisions,  the  IMU  is  modeled  as 

follows : 


0 

0 

by 

0 

0 

bi 

0 

0 

F41 

F42 

F51 

— 

F52 

where : 

2  2 

F41  =  -ws  +  wy  +  uz 
F42  =  -wxwy 
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and 


wy  =  n  cos  *• 

w2  =  n  sin  \ 


» 


27) 
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with 

fi  a  constant  (earth*s  rate  of  rotation) 
\  =  constant  (launch  site  latitude), 

— > 

so  that  w  =  0  and  is  thus  omitted  from  the  F-matrix. 


3.4  FORMULATION  OF  THE  RADAR  AREA  CORRELATION  SYSTEM 


The  description  of  the  radar  area  correlation  system  application 
to  terminal  guidance  is  discussed  in  Appendix  B.  The  radar  system  is 
used  as  an  additional  external  measurement  device  (external  to  the  IMU) 
to  measure  position  in  the  X-Y  plane  defined  by  the  IMU.  The  observa¬ 
tions  or  measurements  for  the  Kalman  filter  are  actually  differences 
between  system- indicated  and  externally  measured  information.  As  is 
common  practice  [ 9] ,  the  measurement  errors  are  attributed  to  inaccu¬ 
racies  in  the  external  indications  only.  Thus,  by  forming  the  differ¬ 
ence  between  the  externally  indicated  and  inertia lly  indicated 
positions 


Z  =  Plnd  (external)  -  Plnd  (IMU) 


=/p  +  e  \ 

I  true  p I 


=  e  -BP 
P 


-  (P„  +  BP) 

true 


(3.28) 


where 

Pind 

P. 

true 

6P 

e 

P 


position  indicated 

true  position  or  errorless  position 

inertial  position  error 

external  device  position  error 


Equation  (3.28)  is  equivalent  to  expressing  the  observation 
equation  as 
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Z(t)  *  [-1  o] 


(3.30) 


'SP(t) 

BV(t) 

L  J 


+  ep(t) 


) 


where  5P  and  6V  are  inertial  errors  in  position  and  velocity  for  this 
exploratory  example. 

The  available  unclassified  information  on  radar  area  correlators 
did  not  delve  into  the  possible  statistical  correlation  in  position 
errors  from  fix  to  fix.  Although  the  fix  to  fix  correlation  seems  a 
distinct  possibility,  the  first  cut  at  a  model  excluded  that  considera¬ 
tion.  In  terms  of  the  state  variables  defined  for  the  IMU,  the  radar 
area  correlation  system's  observation  is  modeled  as 


Z(t)  = 

1  0  0  0  0 

&X 

+ 

/-N 

ft 

0  1  0  0  0_ 

&y 

&z 


(3.31) 


The  values  for  the  radar  system  errors  are  chosen  as  typical  state-of- 

the-art.  Stauffer  [19]  considers  that  to  be  planar  resolution  or,  for 

this  model,  v =  v  =  50  feet  RMS. 
x  y 

Another  consideration  for  the  radar  position  measurement  model  is 
the  effect  of  processing  time  on  the  filter's  performance.  It  was 
learned  that  the  time  delay,  as  it  may  be  considered,  in  matching  the 
reference  imagery  to  the  radar's  projected  real  time  imagery  is 


approximately  0.2  second.  This  indicates  that  by  the  time  a  position 
fix  has  been  processed  by  the  typical  radar  system,  the  information  is 
0.2-second  old,  i.e.,  the  vehicle  has  moved  on.  In  the  case  of  the 
PERSHING,  reentry  at  3000  feet  per  second  the  radar  processing  time 
alone  will  require  a  600- foot  measurement  compensation  in  the  filter 
mechanization.  Intuition  suggests  that  such  an  error  would  eventually 
be  a  source  of  filter  divergence  if  not  properly  addressed. 

This  problem  was  treated  in  the  model  in  two  ways.  First  it  was 
neglected.  This  is  not  as  startling  a  choice  as  might  first  appear. 
There  is  very  little  mention  of  this  delay  phenomenon  in  the  literature 
and  its  effect  on  filter  performance.  It  is  often  mentioned  as  an 
existing  problem  but  is  quickly  discarded  with  the  statement  that 
future  studies  will  be  conducted  in  that  area.  The  best  justification 
for  not  implementing  it  in  this  filter  application  is  that  the  time 
duration  is  so  short  (10  seconds)  that  filter  divergence  will  not 
accrue  a  meaningful  error.  The  results  seem  to  verify  this,  at  least 
in  the  case  where  optimal  gains  are  used. 

Meaningful  fixes  to  this  problem  have  been  proposed.  DeBra  [20] 
has  suggested  that  the  measurement  model  incorporate  the  time  delay  as 
a  nonlinear  exponential  function  with  a  time  constant  comparable  to  the 
best  guess  at  the  time  delay.  For  example,  instead  of  the  linear 
measurement  in  the  variables  5x  and  5y  given  by  Eq.  (3.31),  a  model 
in  the  frequency  domain  is  given  by 

-sT 

Z1(s)  =  e  5x(s)  +  vx(s) 

_sT 

Z2(s)  =  e  fiy(s)  +  vy(s)  ,  (3.32) 
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where  the  time  delay  is  chosen  by  the  value  T.  In  the  t-ime  domain,  two 
additional  state  variables  are  required,  one  to  define  the  time  delay 
for  each  channel.  Thus,  the  F-matrix  would  require  a  two-qtate 
augmentation. 

Bryson  [21]  has  suggested  the  following  methods,  particularly 
applicable  when  the  time  delay  is  not  well  known: 

a)  Increase  the  magnitude  of  appropriate  variance  elements  in 
the  covariance  error  equations 

b)  Increase  the  amplitude  of  the  measurement  noise 

c)  Combination  of  a)  and  b) . 

The  effect  is  to  decrease  the  knowledge  of  the  system  from  the  filter's 
point  of  view. 

The  model  for  the  reentry  vehicle  was  simulated  alternatively  in 
the  manner  of  Bryson.  The  600-foot  error  was  caused  by  processing  time 
was  included  as  additional  error  in  the  measurement  error  covariance 
matrix  R.  Its  effect  was,  therefore,  directly  observable  in  the  cal¬ 
culation  for  the  optimal  filter  gains.  These  results  are  described 
in  Chapter  IV. 

3.5  FORMULATION  OF  THE  RADAR  ALTIMETER  MODEL 

The  radar  altimeter  carried  on  the  PERSHING  will  be  used  to  obtain 
vertical  position  measurements  above  the  earth's  surface.  It  is 
mechanized  in  the  Kalman  filter  formulation  in  exactly  the  same  way  as 
the  radar  area  correlator,  i.e., 


2  =  5PZ  +  epz 


(3.33) 
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Thus,  the  radar  altimeter  is  modeled  as  the  Z-channel  observation 


Z(t)  =  [0  0  1  0  0] 


sy 

&Z 

PV 

y, 


+  vz(t) 


(3.34) 


From  data  on  previous  PERSHING  flights,  the  radar  altimeter  error  is 
known  to  be  approximately  10  percent  of  altitude  indication.  So  that 
an  additional  state  would  not  be  required,  it  was  considered  for 
several  cases  in  this  study  to  be  constant  at  100  feet  and  for  several 
cases,  constant  at  1200  feet.  The  effect  of  the  change  is  discussed  in 
Chapter  IV. 

3.6  A  FILTER  MECHANIZATION  FOR  NONLINEAR  SYSTEMS 

The  filter  equations  developed  by  Kalman  and  Bucy  and  an  extension 
to  correlated  input-measurement  noise  (derived  in  Appendix  C)  were 
developed  under  the  assumption  that  the  system  disturbances  and  the 
measurement  errors  were  random  variables  described  by  Caussian  statis¬ 
tics,  zero  means,  and  that  the  plant  was  describable  by  linear 
equations.  The  resulting  filter  then  was  shown  to  give  the  optimal 
estimate  of  the  states.  Numerous  researchers  in  this  area  have  expanded 
the  ideas  to  the  more  useful  and  practical  case  of  systems  described 
by  nonlinear  dynamical  equations.  For  example,  Bryson  and  Ho  [22], 
in  addition  to  their  own  contributions,  have  a  rich  bibliography  on 
f.i\ese  and  related  topics. 
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The  filter  equations  used  in  this  model  are  of  the  form  most 
applicable  to  the  navigation  problem  at  hand,  i.e.,  a  mixture  of  dis¬ 
crete  and  continuous  equations.  The  discrete  form  is  used  at  a  time 
when  a  new  measurement  is  introduced  and  the  continuous  form  is  used 
to  extrapolate  between  measurements.  Also,  the  equations  are  a  mixture 
of  linear  and  nonlinear  expressions.  The  nonlinear  describe  the  sys¬ 
tem,  i.e.,  the  navigation  system  error  equations,  and  the  linearized 
equations  are  used  for  the  covariance  error  propagation.  These 
equations  are  linearized  about  the  current  estimate  because  in  a 
navigator,  in  general,  and  in  this  model,  in  particular,  there  is  no 
convenient  nominal  path  about  which  to  linearize.  These  equations, 
developed  in  Section  12.6  of  Bryson  and  Ho  [22]  are  summarized  in 
Table  3.2. 

3.7  EQUATIONS  USED  IN  THE  DIGITAL  COMPUTER  SIMULATION 

This  section  summarizes  the  equations  in  the  model  which  were 
used  in  the  Monte  Carlo  simulation.  The  inertial  system  is  modeled  by 
five  error  states;  the  observation  matrix  (H),  models  the  IMU  position 
error  in  three  coordinates  As  measured  by  a  radar  area  correlation 
system  (&x,  5y);  and  a  radar  altimeter  (&z) .  The  initial  conditions, 
error  covariances  and  constants  are  summarized  in  Tables  3.3  through 


3.5. 


Table  3.2 


EXTENDED  KALMAN  FILTER  FOR  NONLINEAR  SYSTEMS  MODELED  IN 


CONTINUOUS  -  DISCRETE  FORM 


Message  model 
(nonlinear) 


Observation  model 
(nonlinear) 


Linearization  about  the 
current  estimate 


x(t)  =  f(x,  u,  t) 


z(t)  -  h(x,  t)  +  v(t) 


F<t)  ,  ,  G(t)  ,  M 


Efu(t))  =  0 


E |u(t) ,  uT(t)|  =  Q5 (t  -  t) 


Filter  algorithm 

between  measurements 


Error  variance  algorithm 
between  measurements 


Filter  algorithm  at  a 
measurement  update 


Error  variance  algorithm 
at  a  measurement 
update 


Optimal  gain  algorithm 
at  a  measurement 
update 


Initial  conditions 


E{v(t) )  =  0 


Ejv(t),  vT(t)|  =  R&(t-i) 


Process  noise 


Measurement  noise 


Efu(t).  v(t ) )  =  0  l  Correlated  process  and 
I  measurement  noise 


x(t)  =  f(x,  t) 


P(t)  =  F(t)  P(t)  +  P(t)  FT(t)  +  G(t)  Q(t)  GT(t) 


K+(t)  =  x_(t)  +  K(t)[z(t)  -  n(x_>  t)J 


P.(t)  =  [  I  -  K(t)  H(t)]  P  (t) 
+  ** 


K(t)  =  P_(t)  HT(t)[H(t)  P_(t)  HT(t)  +  R(t)] 


x  (o)  =  E{x(t0)|=  t.x(o) 

*"0)  =  E  [x(to)-  x(to\]  [x(to)  -  x(to)]  =  ox(o) 
x(o)  =  x^tQ) 


Table  3.3 


SYSTEM  MODEL:  x(t)  =  Fx(t)  +  Gu(t) 


0 

0 

1  ( 

0 

0 

0  ( 

0 

0 

0  1 

L  F42 

F43 

0 

L  F52 

F53 

2ro  < 

y 

2  2 

F4l  =  -  u  +  w  +  m 
s  y  z 

F42  =  -  w  to 
x  y 


F43  =  -  <o  w 
X  z 


F51  •  V’z 

F52  =  -  w„u> 


F53  =  2u  +  w2  +  w2 
s  x  y 


'•'x  =  0  *  !‘’s  "  /”V*e 

(*>  =  0  cos  \ 

u'z  =  (2  sin  \ 


0  =  15.04107  degrees/hour,  G0  =  32.1724  feet/second2 
\  =  45  degrees  north  latitude,  Rg  =  (6,378,388  meters) (3,281  feet/meter) 


u  =  1253  feet 
x 

u  =  1317  feet 

y 

u  =  1500  feet 
z 


fx(0)  =  1253  feet 
f>y(0)  =  1317  feet 
f>z(0)  =  1500  feet 


=  1.2  feet/secord 

u„  =1.4  feet/second 
Vz 


*VX(0)  =  10  feet/second 
FV  (0)  =  10  feet/second 


Table  3.4 

OBSERVATION  MODEL:  z(t)  =  Hx(t)  +  v 


Case  1  (lo) 

Case  2  (la) 

v  =50  feet 

v  =  650  feet 

X 

X 

v  =50  feet 

v  =  650  feet 

y 

y 

v  =  100  feet 

v  =  1200  feet 

z 

z 

Table  3.5 


ERROR  VARIANCE  MODEL:  P(t)  »  FP(t)  +  P(t)F  +  GQG 


P11(0)  =  10,000  feet 
P22(0)  =  10,000  feet2 


P44(0)  =  10  ^  (feet/second)^ 
P^j(O)  =  10  ^  (feet/second) ^ 


P33(0)  =  10,000  feet 


*Initial  conditions  were  obtained  from  reference  [23] 


CHAPTER  IV.  SIMULATION  RESULTS  AND  DISCUSSION 


4.1  OVERVIEW 

The  simulation  of  the  complete  system  modeled  in  the  preceding 
sections  was  performed  on  Stanford's  IBM  360/67  digital  computer. 
Several  variations  were  incorporated  in  the  simulation  to  provide  data 
in  meeting  the  objectives  discussed  in  Chapter  I. 

Initially,  runs  were  made  with  ten  discrete  measurement  updates 
equally  spaced  in  time  (one  every  second).  The  equi-time  spacing 
between  updates  was  chosen  based  on  results  of  Aoki  and  Li  [24] .  The 
Case  I  configuration  used  the  best  information  available;  i.e.,  radar 
measurement  noise  was  limited  to  50-feet  1  sigma  in  the  X  and  Y 
channels  and  to  100- feet  1  sigma  in  the  Z  channel.  These  results  are 
shown  in  Figs.  4.1  through  4.5.  The  optimal  time  varying  gains  were 
then  observed  from  this  data,  Figs.  4.6  through  4.10,  and  were  used  to 
mechanize  a  suboptimal  filter  with  fixed  ga*ns.  Those  results  were 
shown  in  Figs.  4.11  through  4.14. 

Then  a  simulation  was  performed,  similar  in  every  respect  to  the 
previous  one  except  that  radar  measurements  were  decreased  to  5;  i.e., 
there  was  a  measurement  update  once  every  2  seconds  in  the  10-sccond 
simulated  reentry.  Again,  the  optimal  gains  were  computed  and  then  the 
results  were  used  to  mechanize  a  suboptimal  fixed  gain  filter.  These 
results  are  shown  in  Figs.  4.15  through  4.25. 
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Additionally,  a  set  of  data  were  obtained  from  the  Case  II  configu¬ 
ration  with  five  measurement  updates.  Recall  Case  II  used  the  degraded 
measurement  information  in  terms  of  increased  covariance  errors  and 
measurement  noise.  The  values  of  650- feet  1  sigma  in  the  X  and  Y 
channel  and  1200- feet  1  sigma  in  the  Z  channel  were  used  as  measurement 
noise.  These  results  are  show?*  Figs.  4.32  through  4.38. 

There  were  other  sensitivity  checks  made  in  this  study.  Although 
no  graphs  were  plotted  they  represent  additional  results.  A  case  of 
twenty  updates  with  fixed  gains  was  simulated  and  several  cases  with 
interchanged  elements  in  the  Case  I  and  Case  II  configurations  were 
also  obtained  and  shown  as  Figs.  4.39  and  4.40.  These  results  are 
discussed  in  the  following  paragraphs  of  this  chapter. 

4.2  TEN  MEASUREMENT  UPDATES,  OPTIMAL  GAINS.  CASE  I  STATISTICS 

The  results  indicating  performance  of  the  filter  for  this  case 
are  shown  in  Figs.  4.1  through  4.5.  Each  figure  plots  the  value  of 
±1  standard  deviation  from  the  indicated  covariance  matrix  value. 

That  is,  each  graph  representing  one  error  state  of  the  IMU  has 
the  square  root  of  the  term  in  the  diagonal  of  the  covariance  matrix 
plotted  as  a  positive  and  negative  1  sigma  value.  Because  Gaussian 
statistics  are  used  with  zero  mean  values,  the  positive  standard  devia¬ 
tion  (plus  1  sigma  value)  may  be  interpreted  as  the  root  mean  square 
(RMS)  error  in  estimating  the  applicable  state  variable.  Additionally, 
each  figure  shows  the  difference  between  the  actual  state  and  the  best 
estimate  of  it  (&x  -  Sx)  which  is,  in  fact,  the  estimation  error  of  the 
filter.  One  way  of  interpreting  the  results  is  to  observe  that  the 
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FIG  4.1.  FILTER  ERROR  FOR  OPTIMAL  ESTIMATION  OF  THE  IMU'S 

X  POSITION  ERROR  WITH  CASE  I  MEASUREMENT  STATISTICS 
AND  TEN  UPDATES 


FIG  4.2.  FILTER  ERROR  FOR  OPTIMAL  ESTIMATION  OF  THE  IMU'S 

Y  POSITION  ERROR  WITH  CASE  I  MEASUREMENT  STATISTICS 
AND  TEN  UPDATES 


TIME  (mc) 

FIG  4.3.  FILTER  ERROR  FOR  OPTIMAL  ESTIMATION  OF  THE  IMU'S 

Z  POSITION  ERROR  WITH  CASE  I  MEASUREMENT  STATISTICS 
AND  TEN  UPDATES 


TIME  (mc) 

FIG  4.4  FILTER  ERROR  FOR  OPTIMAL  ESTIMATION  OF  THE  IMU*S 

Vx  VELOCITY  ERROR  WITH  CASE  I  MEASUREMENT  STATISTICS 

AND  TEN  UPDATES 


FIG  4.5.  FILTER  ERROR  FOR  OPTIMAL  ESTIMATION  OF  THE  IMU'S 

V  VELOCITY  ERROR  WITH  CASE  I  MEASUREMENT  STATISTICS 
z 

AND  TEN  UPDATES 

error  (the  irregular  or  "noise-like"  trace)  should  be  within  the  ±1 
sigma  curves  approximately  63  percent  of  the  time  if  the  filter  is 
performing  properly. 

The  time  optimal  gains  generated  by  the  filter  are  shown  in  Figs. 
4.6  through  4.10.  These  are  displayed  so  that  the  fixed  gains  chosen 
for  the  suboptimal  filter  can  be  readily  compared. 

4.3  TEN  MEASUREMENT  UPDATES.  FIXED  GAINS.  CASE  I  STATISTICS 

The  fixed  gains  are  chosen  to  closely  approximate  the  optimal 
gains.  They  can  not  be  properly  selected  without  having  computed  the 
optimal  solution  first.  For  display  purposes,  the  optimal  gains  are 
shown  with  the  fixed  gains  so  that  a  visual  comparison  can  be  made. 
These  are  also  shown  in  Figs.  4.6  through  4.10, 
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0.60 


OPTIMAL  GAIN 


FIG  4.6.  COMPARISON  OF  SUBOPT IMAL  AND  OPTIMAL  TIME  VARYING 
GAIN  Kx  FOR  CASE  I  MEASUREMENT  STATISTICS  WITH  TEN 

UPDATES 


FIG  4.7.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 
GAIN  Ky  FOR  CASE  I  MEASUREMENT  STATISTICS  WITH 

TEN  UPDATES 


TIME  (sec) 

FIG  4.8.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 

GAIN  K  FOR  CASE  I  MEASUREMENT  STATISTICS  WITH 
z 

TEN  UPDATES 


TIME  (sec) 


FIG  4.9.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 
GAIN  FOR  CASE  1  MEASUREMENT  STATISTICS  WITH 

TEN  UPDATES 


FIG  4.10.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 

GAIN  K  FOR  CASE  I  MEASUREMENT  STATISTICS  WITH 
vz 

TEN  UPDATES 

The  error  in  the  filter  in  attempting  to  estimate  the  IMU  error, 
is  shown  in  Figs.  4.11  through  4.14.  It  is  evident  that  the  filter 
with  fixed  gains  does  not  estimate  the  states  as  well  as  the  filter 
with  optimal  gains.  It  is,  in  fact,  divergent  in  some  ea*  >s.  Several 
reasons  are  available  to  explain  this  phenomenon.  These  are  discussed 
more  completely  in  Chapter  4.9. 

4.4  FIVE  MEASUREMENT  UPDATES.  OPTIMAL  GAINS.  CASE  I  STATISTICS 


The  format  of  the  graphical  data  is  similar  to  the  case  for  ten 
measurement  updates.  To  minimize  data  presentation  which  may  appear 
repetitious  (it  is  not)  and  to  enhance  the  comparison,  the  covariance 
error  data  represented  as  the  positive  standard  deviation  is  plotted  in 
Figs.  4.15  and  4.16.  The  same  comparison  is  done  for  the  filter's 
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FIG  4.13.  FILTER  ERROR  FOR  SUSOPTIMAL  ESTIMATION  OF  THE  IMU'S 

V  VELOCITY  ERROR  WITH  CASE  I  MEASUREMENT  STATISTICS 
x 

AND  TEN  UPDATES 


.IME  (tec) 


FIG  4.14.  FILTER  ERROR  FOR  SUBOPTIMAI  ESTIMATION  OF  THE  IMU'S 

V  VELOCITY  ERROR  WITH  CASE  I  MEASUREMENT  STATISTICS 
z 

AND  TEN  UPDATES 


Z  (x  to*01)  (ft)  Y  (x  10*01)  (ft)  X  (x  10+01)  (ft) 


FIG 


.  4.15.  COMPARISON  OF  INU'S  RMS  POSITION  ERROR  HISTORIES 
USING  OPTIMAL  GAINS  WITH  10  POSITION  FIX  ERRORS 
OF  50- FOOT  RMS  IN  X  A NO  Y  AND  100- FOOT  RMS  IN  Z 


240 


FIVE  UPDATES 


FIG  4.16.  COMPARISON  OF  IMU'S  RMS  VELOCITY  ERROR  HISTORIES 
USING  OPTIMAL  GAINS  WITH  TEN  POSITION  FIX  ERRORS 
OF  50-FOOT  RMS  IN  X  AND  Y  AND  lOO-FOOT  RMS  IN  Z 


error  in  the  estimate  of  the  IMU  error  states  in  Figs.  4.17  through 
4.20.  As  can  be  seen,  there  is  better  performance  from  the  filter  in 
the  case  vhere  ten  updates  are  used. 

4.5  FIVE  MEASUREMENT  UPDATES.  FIXED  GAINS.  CASE  I  STATISTICS 

The  fixed  gains  compared  to  the  optimal  gains  are  given  in  Figs. 
4,21  through  4.25.  Because  of  the  change  in  the  graphs'  ordinate 
scale  the  direct  comparison  to  the  ten  measurement  case  is  not  possi¬ 
ble  to  display.  However,  again  it  is  noted  that  the  filter  perfor¬ 
mance  is  poorer  when  the  number  of  measurements  is  decreased. 
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FIG  4.19.  COMPARISON  OF  FILTER  ERROR  FOR  OPTIMAL  ESTIMATION 

Of  THE  IMU'S  Z  POSITION  ERROR  WITH  CASE  I  STATISTICS 


TIME  (wc) 


FIG  4.20.  COMPARISON  OF  FILTER  ERROR  FOR  OPTIMAL  ESTIMATION 

OF  THE  IMU'S  V  AND  V  VELOCITY  ERRORS  WITH  CASE 
x  z 


I  STATISTICS 


FIG  4.21.  COMPARISON  OF  SUBOFTIMAL  AND  OPTIMAL  TIME  VARYING 
GAIN  Kx  WITH  CASE  I  MEASUREMENT  STATISTICS  AND 

FIVE  UPDATES 


FIG  4.22.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 
GAIN  K  WITH  CASE  I  MEASUREMENT  STATISTICS  AND 

y 

FIVE  UPDATES 
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FIG  4.23.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 
GAIN  Kz  WITH  CASE  I  MEASUREMENT  STATISTICS  AND 

FIVE  UPDATES 
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FIG  4.24.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 
GAIN  K  WITH  CASE  I  MEASUREMENT  STATISTICS 

VX 
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FIG  4.25.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 

GAIN  K  WITH  CASE  I  MEASUREMENT  STATISTICS  AND 
vz 

FIVE  UPDATES 

The  filter  errors  are  shown  in  Figs.  4.26  through  4.29.  They  are  also 
seen  co  diverge  and  for  the  same  reasons  as  in  the  ten  measurement 
fixed  gain  case. 
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FIG  4.27.  FILTER  ERROR  FOR  SUBOPTIMAL  ESTIMATION  OF  THE  IMU'S 

V  AND  V  VELOCITY  ERRORS  WITH  CASE  I  MEASUREMENT 
x  z 
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FIG  4.28  COMPARISON  OF  FILTER  ERROR  FOR  OPTIMAL  ESTIMATION 
OF  THE  IMF'S  X  AND  Y  POSITION  ERRORS  WITH  FIVE 
UPDATES 


4.6  FIVE  MEASUREMENT  UPDATES.  OPTIMAL  GAIKS.  CASE  II  STATISTICS 


The  filter  errors  for  this  case  are  shown  plotted  against  the 

filter  errors  for  optimal  gains,  Case  I.  Case  II  differs  from  Case  I 

in  measurement  noise  parameters.  In  Case  II,  V  and  V  are  650- foot 

x  y 

root  mean  square  compared  to  50- feet  PMS  for  CASE  I.  Also,  v  is 

larger  at  1200- foot  RMS  compared  to  vz  of  Case  I  which  is  100- foot  RMS. 

The  increase  in  measurement  error  parameters  reflect  the  attempt  to 

include  the  error  caused  by  radar  area  correlator  time  delay  that 

occurs  while  obtaining  a  position  measurement.  The  1100- foot  increase 

in  v  measurement  error  is  used  to  obtain  another  set  of  results  by 
z 

using  a  fixed  error  in  altitude.  In  reality,  the  actual  radar  altim¬ 
eter  error  is  10  percent  of  the  indicated  value. 

The  results  shown  in  Figs.  4.28  and  4.29  verify  that  the  filter 
errors  are  smaller  without  consideration  of  the  time  delay;  however, 
care  must  be  taken.  The  truer  more  realistic  case  is  given  by  the 
larger  filter  error.  There  can  not  be  enough  emphasis  placed  on  the 
statement  that  the  filter  is  only  as  good,  at  best,  as  the  model  used 
to  describe  the  real  system.  Because  the  model  used  for  this  study  can 
never  be  completely  defined  to  represent  an  actual  system,  the  filter's 
performance  will  vary  according  to  the  information  mathematically 
included  in  .ts  make  up. 

The  ±1  sigma  values  rf  the  expected  error  are  shown  in  a  compara¬ 
tive  display  of  the  Case  I  and  Case  II  results.  Again,  as  expected, 
a  larger  measurement  error  yields  a  larger  standard  deviation.  These 
are  shown  in  Figs.  4.30  and  4.31. 
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FIG.  4.30.  COMPARISON  OF  IMU'S  RMS  POSITION  ERROR  HISTORIES 
USING  OPTIMAL  GAINS  WITH  FIVE  POSITION  FIX  ERRORS 
OF  6 50- FOOT  RMS  IN  X  AND  Y  AND  1200-FOOT  RMS  IN  Z 


4.7  FIVE  MEASUREMENT  UPDATES.  FIXED  GAINS.  CASE  II  STATISTICS 

The  gains  of  the  optimal  filter  are  shown  with  the  fixed  gains 
chosen  to  mechanize  the  suboptimal  filter  of  Case  II.  These  are  given 
as  Figs.  4.32  through  4.36.  The  Case  II  optimal  gains  are  shown  with 
them.  The  results  of  the  Case  II  fixed  gains  are  plotted  with  the 
standard  deviations  and  the  filter  errors  in  Figs.  4.37  and  4.38. 

These  errors  in  estimating  the  states  are  larger  than  the  Case  I 
results  seen  earlier. 
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FIG.  4.31.  COMPARISON  OF  IMU’S  RMS  VELOCITY  ERROR  HISTORIES 

USING  OPTIMAL  GAINS  WITH  FIVE  POSITION  FIX  ERRORS  OF 
650-FOOT  RMS  in  X  and  Y  AND  1200-FOOT  RMS  IN  Z 


4. 8  ADDITIONAL  RESULTS 

During  the  course  of  this  study,  several  changes  were  made  to  the 
model.  As  was  mentioned  earlier,  the  problem  of  the  negative  definite 
error  covariance  matrix  was  investigated  and  corrected  with  a  method 
that  proved  successful  in  obtaining  the  plots.  In  one  instance, 
however,  a  different  method  of  fix  was  used.  The  particular  simulation 
was  performed  with  fixed  gains  obtained  from  the  optimal  ten  measure¬ 
ment  update  results .  (Recall  that  only  in  the  fixed  gain  runs  did  the 
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FIG.  4.32.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 
GAIN  Kx  WITH  CASE  II  MEASUREMENT  STATISTICS  AND 

FIVE  UPDATES 


FIG.  4.33.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 
GAIN  Xy  WITH  CASE  II  MEASUREMENT  STATISTICS  AND 

FIVE  UPDATES 


negative  definite  covariance  appear.)  To  overcome  the  divergence,  a 
smaller  integration  step  size  was  chosen,  0.05  second  compared  to  0.2 
second,  and  the  number  of  measurement  updates  was  increased  from  10  to  . 
20.  Though  not  displayed  here,  the  results  indicated  that  the  covari¬ 
ance  matrix  become  positive  semidefinite .  The  filter  error  was  approx¬ 
imately  10-percent  smaller  than  the  fixed  five  and  the  fixed  ten  update 
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FIG  4.34.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 

GAIN  K  WITH  CASE  II  MEASUREMENT  STATISTICS  AND  FIVE 
z 

UPDATES 


FIG  4.35.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 
GAIN  Kvx  WITH  CASE  II  MEASUREMENT  STATISTICS  AND 

FIVE  UPDATES 


FIG  4.36.  COMPARISON  OF  SUBOPTIMAL  AND  OPTIMAL  TIME  VARYING 

GAIN  K  WITH  CASE  II  MEASUREMENT  STATISTICS  AND 
vz 

FIVE  UPDATES 
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cases.  However,  this  filter  also  began  to  diverge  near  the  last 
several  seconds  in  much  the  same  manner  as  the  other  fixed  gain  cases. 
From  the  trend  observed  on  several  states,  a  simulated  flight  of 
larger  than  10  seconds  may  have  eventually  resulted  in  all  of  the 
error  state  estimates  diverging. 

Another  case  was  run  in  which  the  measurement  noise  was  increased 
to  reflect  a  Case  II  situation  but  the  R-matrix  values  were  the  reduced 
values  of  the  Case  I  situation.  This  was  for  a  five  measurement  update 
fixed  gains  simulation  with  gains  chosen  from  the  Case  I  results. 

There  was  virtually  no  noticeable  change  from  the  straight  Case  I 
results  in  the  plots.  The  results  were  exactly  the  same  as  those 
displayed  in  Figs.  4.11  through  4.14. 

Pursuing  this  one  more  step-,  additional  runs  were  made  but  the 
R-matrix  values  were  increased  to  fully  reflect  the  Case  II  situation. 
Once  again,  however,  the  fixed  gains  were  chosen  from  Case  I.  Again, 
the  results  obtained  were  almost  exactly  these  of  Case  I.  The  conclu¬ 
sion  is  inescapable,  the  filter  is  not  sensitive  to  measurement  error 
and  measurement  error  covariance  matrix  changes  when  the  gains  are 
fixed.  The  filter  is  a  function  of  the  gains  alone  in  the  fixed  gain 
mode.  Different  results  are  obtained  when  the  same  simulation  is  run 
with  the  exception  that  fixed  gains,  more  accurately  reflecting  Case 
II,  are  chosen.  Those  results  are  in  Figs.  4.39  and  4.40  and  are 
different  from  the  Case  I  fixed  gains  with  Case  II  statistics. 

4.9  DISCUSSION  OF  RESULTS 

The  error  of  the  filter  in  estimating  the  IMU  error  states  was 
shown  to  be  larger  in  every  case  where  the  fixed  gains  were  used.  In 
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FIG  4.39.  FILTER  ERROR  FOR  SUBOPT IMAL  ESTIMATION  OF  THE 
IMU'S  X  AND  Y  POSITION  ERRORS  WITH  CASE  II 
MEASUREMENT  STATISTICS  AND  CASE  I  FIXED  GAINS 
USING  FIVE  UPDATES 
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FIG  4.40.  FILTER  ERROR  FOR  SUBOPT IMAL  ESTIMATION  OF  THE  IMU'S 
V  AND  V  VELOCITY  ERRORS  WITH  CASE  II  MEASUREMENT 

X  2 

STATISTICS  AND  CASE  I  FIXED  GAINS  USING  FIVE  UPDATES 


some  instances,  there  were  divergent  estimates  whereas  the  optimal 
gains  did  not  exhibit  this  behavior.  Qualitatively,  the  following 
argument  seems  reasonable. 

The  estimate  of  the  IMU  error  state  depends  on  the  difference  in 
the  actual  measurement  vector  (2)  and  the  knowledge  of  the  measurement 
matrix  (H)  with  the  estimate  of  the  state  vector  (X)  at  the  instant  the 
measurements  are  taken.  This  difference  is  multiplied,  or  weighted, 
by  the  optimal  gain  (K)  which,  in  turn,  is  a  function  of  terms  computed 
from  the  covariance  differential  equation  (P) .  In  the  case  where  the 
optimal  gains  are  used,  the  value  of  K  is  computed  at  every  measurement 
update  and  is  a  function  of  the  measurement  noise,  observation  matrix, 
and  more  importantly,  the  old  covariance  values  (P  ).  The  covariance 
P_  is  obtained  from  a  continuing  propagation  of  P  between  measurements. 
The  optimal  gain  (K)  then  utilizes  all  the  measurement  noise  informa¬ 
tion,  as  well  as  process  noise  information,  and  may  grow  or  decrease  as 
the  equations  dictate.  The  gain  values  (K)  are  essentially  the  ratio 
between  statistical  measures  of  uncertainty  of  the  state  estimate  and 
uncertainty  in  a  measurement.  If  measurement  noise  is  large  and  state 
estimate  errors  are  small,  the  error  in  the  measurement  vector  is 
caused  chiefly  by  noise  and,  therefore,  only  a  small  change  in  the 
state  estimate  would  be  made;  i.e.,  K  will  be  small.  However,  small 
measurement  noise  and  large  uncertainty  in  the  state  estimate 
indicates  that  the  measurement  vector  contains  a  large  quantity  of 
information  on  the  errors  in  the  estimates.  Thus,  the  value  for  K  will 
be  large  because  the  difference  between  the  actual  measurement  and 
that  predicted  from  HX  would  be  used  as  the  basis  for  a  heavily 
weighted  correction  to  the  estimates. 


Simply  stated,  when  fixed  gains  are  used,  the  previously  described 
rationale  does  not  occur.  The  covariance  differential  equation  is  not 
propagated  between  measurements  (the  single  most  important  reason  for 
choosing  a  set  of  fixed  gains)  so  that  all  process  noise  statistics 
are  ignored.  Because  K  is  fixed,  there  is  no  dependence  update  to 
update,  on  the  measurement  matrix  (H)  nor  on  noise  statistics 
contained  in  the  R  matrix.  Little  significance  is  placed  on  the  new 
incoming  data,  via  the  measurements  Consequently,  when  the  measure¬ 
ment  vector  has  good  data,  it  may  be  ignored  and  the  error  in  the 
estimated  states  continues  to  grow  or  diverge. 

It  should  be  emphasized  that  on  an  actual  on-board  computer 
utilizing  the  fixed  gains  to  implement  the  filter,  the  covariance 
matrix  Riccati  equations  (P)  would  not  be  computed.  That  they  are  not 
to  be  computed  on  board  is  the  motivation  for  studying  the  effects  of 
using  fixed  gains.  In  this  study  however,  the  plots  of  the  standard 
deviation  calculated  from  the  covariance  matrix  were  computed  with 
fixed  gains  for  comparison  purposes.  This  was  done  to  reinforce  the 
arguments  of  defining  a  filter  with  good  performance  versus  one  which 
tended  to  diverge.  As  can  be  observed  from  the  results  presented,  as 
the  covariance  increases,  the  filter's  error  increases. 

A  final  comment  on  the  fixed  gain  results  is  worthy  of  mention. 

The  covariance  matrix,  which  theoretically  will  be  positive  semidefi- 
nite,  became  negative  definite  in  the  or  &z  term.  This  is  the 
classic  effect  of  filter  divergence  discussed  in  much  of  the 
literature.  Because  there  was  a  need  to  obtain  the  square  root  of 
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this  matrix  in  the  plot  subroutine,  termination  of  the  program  occurred 
before  all  the  plots  could  be  made.  The  problem  was  overcome  by  not 
plotting  the  filter  error  covariance  in  the  estimate  of  &z.  This 
occurred  in  every  fixed  gain  simulation  except  one  when  another  method 
was  used  to  correct  the  problem.  This  particular  case  was  discussed 
in  Chapter  4.8. 
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CHAPTER  V.  CONCLUSIONS  AND  RECOMMENDATIONS  FOR  FUTURE  STUDY 


5.1  CONCLUSIONS 

In  terms  of  the  objectives  specified  in  Section  1.5,  the  following 
conclusions  are  stated  as  having  been  verified  during  the  course  of 
this  study. 

a)  The  simplest  inertial  system  mechanization  which  fulfills 
the  constraint  of  minimum  on-board  computer  capacity  is  the  tangent 
plane  mechanization. 

b)  The  comparison  between  results  of  filters  using  five  measure¬ 
ments  updates  and  ten  measurement  updates  indicate  the  filter  error  is 
smaller  with  more  updates  in  estimating  the  state  variables.  The 
choice  of  ten  measurement  updates  giving  better  results  is  shown  con¬ 
clusively.  Indications  are  that  the  maximum  of  15  measurements  allow¬ 
able  would  be  the  best.  The  measurements  must  bn  equally  spaced  in 
time  for  this  application  to  allow  time  for  processing. 

c)  The  filter  should  be  formulated  with  no  less  than  five  state- 
variables.  If  the  on-board  computer  has  the  capacity,  the  additional 
three  states  describing  a  constant  inertial  platform  tilt  would  be 
desirable  provided  statistics  describing  IMU  tilt  can  be  obtained  so 
that  initial  conditions  can  be  properly  chosen. 

d)  The  complete  five  state  filter  studied  here  should  not  be 
formulated  with  fixed  gains  in  each  of  the  state  estimates.  The 


results  do  Indicate  however,  that  the  IMU  position  errors  in  X  and 
Y  are  estimated  with  only  modest  filter  error.  Thus,  the  X  and  Y 
position  errors  can  be  derived  in  a  fixed  gain  formulation.  The 
savings  in  computer  capacity  needed  to  estimate  three  states  with 
optimal  time  varying  gains,  while  using  fixed  gains  in  estimating  the 
other  two  states,  alone  yield  a  savings  of  40  percent  in  computing  the 
covariance  matrix  Riccati  equation. 

e)  An  excellent  reference  [17]  was  obtained  which  gave  a  compre¬ 
hensive  table  of  IMU  characteristics  required  for  performance  with  the 
statistics  used  in  this  study.  It  is  used  to  specify  the  IMU  and  por¬ 
tions  of  the  IMU  computer  for  physical  realization.  Table  5.1  summa¬ 
rizes  the  IMU  specifications.  Specification  for  the  radar  area  corre¬ 
lation  system  and  radar  altimeter  are  more  general.  The  figure  of 
merit  used  for  the  radar  area  correlation  system  was  its  resolution 
and  for  the  altimeter,  its  accuracy.  These  are  summarized  in 
Table  5.2. 

5.2  RECOMMENDATIONS  FOR  FUTURE  STUDY 

There  are  several  problems  related  to  this  application  which 
could  be  pursued  further.  The  first  of  these  would  be  to  include  more 
states  in  the  filter  formulation.  This  would  more  closely  represent 
the  physical  situation,  though  it  would  require  more  computer  capacity 
than  may  be  allowed.  The  purpose  would  be  to  investigate  the  filter 
error  relative  to  the  number  of  states  modeled.  It  would  not  imply  a 
priori  that  additional  states  need  be  used  in  the  actual  system.  The 
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Table  5.1 


SPECIFICATIONS  FOR  THE  IMU 


Gyros 

Magnitude 

Component 

Specifications 

Constant  drift  rate 
(degree/hour) 

0.3 

Mass  unbalance 
[  (degree/hour ) /g] 

0.2 

End  plate  drift 
[  ( degree/hour )/g] 

0.15 

Anisoelasticity  ^ 
f (degree/hour) /gZ] 

— 

Accelerometers 

Magnitude 

Bias  (g) 

io~4 

Scale  factor  (g/g) 

81  x  10"6 

2 

Nonlinearity  (g/g  ) 

— 

Allowable  Misalignment 

1  Sigma  Error 

Placform 

Specification 

X-Y  plane  level 
(arc  seconds) 

26.7 

Azimuth  (arc  seconds) 

32.4 

Error  Source 

1  Sigma  Error 

Guidance 

Computer 

In-flight  errors 
(meter/second) 

0.15 

Displacement  error 
(meter) 

10.0 

Velocity  error 
(meter/second) 

0.4 

following  considerations  should  also  be  investigated  to  observe  the 
effects  on  the  filter  error: 

a)  The  radar  area  correlation  system  is  considered  to  have 
measurement  noise  correlated  with  the  input  noise 

b)  The  radar  area  correlation  system's  time  delay  is  modeled 


as  a  true  transport  lag 


Table  5.2 
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without  computing  the  covariance  matrix  Riccati  equation.  That  is, 
instead  of  choosing  a  two  level  value  of  K,  choose  it  to  be  five  level 
for  the  five  update  case  and  ten  level  for  the  ten  update  case.  In  the 
latter  for  example,  storing  50  values  of  K,  10  for  each  of  the  5 
states,  would  be  a  savings  on  the  computer  required,  presuming  the 
filter  does  not  diverge. 

An  analysis  should  be  performed  to  determine  the  sensitivity  of 
the  filter  to  the  simplification  of  using  fixed  or  precomputed  gains. 

It  was  observed  that  the  accuracy  of  the  filter  degraded  when  the  fixed 
gains  were  used.  This  analysis  would  give  the  bounds  and  structure  of 
the  error  covariance  as  a  function  of  gain  using  fixed  noise  statis¬ 
tics.  It  would  lend  some  confidence  to  the  greater  use  of  fixed  or 
precomputed  gains  stored  a  priori. 

A  complete  trajectory  study  is  characterized  by  its  higher  com¬ 
plexity  relative  to  the  case  presented  in  this  thesis .  It  includes  a 
full  aerodynamic  description  of  the  flight  vehicle,  its  inertia  proper¬ 
ties,  autopilot  mechanization,  and  targeting  information  in  addition 
to  the  measurement  kinematics  considered.  Then,  by  appropriate  mani¬ 
pulation  of  initial  conditions,  the  truest  figure  of  merit,  the 
vehicle's  miss  distance,  could  be  established  and  compared  in  cases 
with  and  without  the  filter  implemented.  The  concept  would  be  simu¬ 
lated  with  a  complete  inertial  system,  radar  area  correlation  system, 
and  radar  altimeter  in  the  autopilot  mechanization.  The  estimator 
would  still  be  used  to  feed  back  error  states  to  the  inertial  system 
for  correction  of  its  output  to  the  autopilot  actuators.  In  proper 
perspective,  it  should  be  noted  that  this  entire  report  would  be  the 
basis  of  only  one  subroutine  (the  estimator)  in  such  a  simulation. 
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Needless  to  say,  this  type  o£  study  is  a  quantum  step  up  in  the 
hierarchy  of  analysis  and  simulation.  But  by  the  very  virtue  of  its 
complexity,  it  represents  the  best  tool  closely  approaching  actual 
hardware  flight  test.  The  only  additional  realism  would  be  to  include 
a  hardware- in- the- loop  simulation.  However,  the  obvious  disadvantages 
of  hardware  acquisition  (caused  by  high  cost  and  lack  of  availability) 
and  maintenance,  preclude  it  from  serious  consideration  at  this  time. 
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APPENDIX  A 

DERIVATION  OF  THE  IMU  POSITION  AND  PLATFORM 
MISALIGNMENT  ERROR  EQUATIONS 

A.l  PLATFORM  MISALIGNMENT  ERROR  EQUATION 

It  is  assumed  that  the  computer  mechanization  is  perfect,  i.e., 
that  the  equations  of  motion  are  solved  with  accuracy.  Thus,  the 
guidance  system  would  operate  perfectly  if  the  initial  conditions  were 
correct  and  if  there  were  no  component  errors.  Realistically,  there 
are  a  host  of  errors  contributed  by  the  gyros,  accelerometers, 
resolvers,  torquers,  pick-offs,  etc.  However,  this  analysis  will  only 
consider  two  major  errors  that  the  gyro  and  accelerometer  propagate. 

The  predominant  sources  of  error  for  the  gyro  are  the  drift  rate  and 
scale  factor,  and  for  the  accelerometer  the  bias  and  scale  factor. 

Three  coincident  coordinate  axes  are  of  interest.  Each  is  defined 
by  a  set  of  orthogonal  unit  vectors  in  a  right  handed  triad.  For  a 
perfectly  operating,  errorless  guidance  system,  all  three  bases  would 
coincide.  For  small  angular  rotations,  a  pseudovector  may  be  defined 
which  is  the  vector  angle  relating  one  basis  to  another.  It  can  there¬ 
fore  be  defined  by  the  following: 

A 
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51 


61 


the  vector  angle  between  the  computer  basis  and  the 
platform  basis. 
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the  vector  angle  between  the  local  basis  (which  may 
be  in  any  mechanization)  and  the  platform  basis. 
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A  the  vector  angle  between  the  local  basis  and  the 
computer  basis. 


As  mentioned  previously,  ideally  all  the  bases  would  coincide;  but  by 
the  definitions  given,  it  is  concluded 
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bt 


(A.l) 


In  terms  of  the  notation  used  throughout  this  thesis, 

.  ->P-l 
5<t>  £  U 

— >C-L 
60  A  to 

e->  .  ->P-C 

A  to  . 

Thus,  Eq.  (A.l)  is  rewritten  as 


-♦P-L  — *P“C  ,  — »C“  L 

0)  e  (0  +  <0 


(A. 2) 


Ideally,  it  is  desired  that  the  platform  rotate  with  the  local 
coordinate  basis  in  inertial  space,  i.e,, 


-d?-I  ->L-I 

0)  «  w 


(Ideal) 


(A.  3) 


However,  because  the  gyros  are  measuring  this  platform  rotation  with 
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respect  to  the  inertial  basis  and  because  the  gyros  are  additionally 
being  torqued  by  signals  from  the  computer  to  maintain  a  particular 
mechanization,  the  platform  conforms  to  the  following  actual  matrix 
equation: 


where 


(■•  *  :•) 


J-1  =  |E„  4  K 
P 


C  «*T 

w  +  e  ,  (Actual) 
C  P 


(A  .4) 
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To  get  all  terms  in  the  same  basis,  note  that  the  following  transforma¬ 
tions  hold: 


and 


where 


Thus, 


w  =  Tp/_  w 
P  F/C  C 


TP/C  =  *J/C  -  E3  +  * 


T  = 


-f. 


■f. 


t 


c-x  C-I  C-I  ,  w  C-I 

w  =  (E„  +  T)  to  =  u  +  ¥  to 

C  '  J  '  P  P  P 


(A. 5) 


(A  .6) 


(A. 7) 
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Putting  Eq.  (A. 7)  into  Eq.  (A .4)  yields 


(E3  +  ltg)(“C'1  +  51 


=  f-1  +  K  S'1  +  *  /-1  ♦  E 


P  p6  P 


P  P 


(A. 8) 


thus, 
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(A.9): 


-»P~I  -iC-I  -3  -*C-I  ,  -vC-1  -» 

w  =  w  +  K  •  oj  +  x  u  +  e 


(A.  10) 


->P-I  -»C-I  -»p-c 

u>  “  w  =  u 


where  from  the  earlier  definitions, 


— >P“C  A 

w  A  5ijf 


It  is  also  noted  that 


.  C 

A  — > 

a\,  & 


(A.  11) 


Combining  Eq.  (A. 10)  and  Eq.  (A. 11)  yields  the  following: 


?=i£  .  ~f‘l  +Vxtlc"1  +  6 


(A.  12) 
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Finally,  as  a  minor  modification,  let  50  =  0;  i.e.,  let  the  ideal  local 
frame  equal  the  ideal  computer  frame.  The  resulting  local  frame  sym¬ 
bols  are 

(A.  13) 

Eq.  (A. 13)  is  the  same  as  Eq.  (2.9)  in  the  text. 

A. 2  POSITION  ERROR  EQUATION 

Beginning  with  the  set  of  equations  in  invariant  vector  form  given 
as  Eqs.  (2.8a)  and  (2.8b),  assume  that  50  =  0;  i.e.,  that  the  local 
basis  is  aligned  with  the  true  or  computer  basis.  Thus, 

?=  V  +lf"C  X  ?  (A.  14) 

and  . 

=  f  +  g  -  (if"1  +  if”1)  X  V  .  (A. 15) 

Equation  (A. 15)  which  is  in  terms  of  the  gravity  field  intensity  vec¬ 
tors  is,  when  rewritten  in  terms  of  the  gravitational  field  intensity 
vectors, 

V  =  f  +  G  -  (if”1  +1)C”1)  x  V  -I)2"1  x(t>E_I  X  r)  .  (A.  16) 

Now  defining  the  indicate  quantities  in  the  preceding  ideal  equation 
composed  of  a  nominal  component  and  an  error  component,  the  following 
variation  to  first  order  is  resolved: 

c  c 

r  +  5r  =  V  +  6V  -  u  x  r  -  w  x  &r  ,  (A*17) 
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where 


— i  C  -E  — >E-C 

(J  s  -  U) 


_>C-E  _*C-I  _>E-I 

(J  =  td  -  U) 


,  ~>C-I 
and  u 


,  -»E-I 
and  u 


are  known  exactly  (no  perturbation  is  needed) . 


Subtracting  Eq.  (A. 14)  from  Eq.  (A. 17)  yields 


C 

&r 


=  &V  -  a) 


C-E 


X  &r 


(A. 18) 


rewritten  as 


C 

8V  =  5?  +~u>C~E  X  &1?  ,  (A. 19) 


whi'-n  is  recognized  as  the  Coriolis  Law  if  the  following  definition  is 
used: 


Thus, 


E 

6V*  A  &r 


->  §♦  £  -♦C-E 

8V  =  &r  =  &r  +  ti)  x  &r 


Now  by  straight  forward  application  of  the  Coriolis  Law 


6r  =  5?  +t.E'1  X  8r 
=  &V  +'c5E,’:C  X  8r 

Taking  another  derivative, 

br  =  6V  +t)E"1  X  6r 


(A.  20) 


(A. 21a) 
(A. 21b) 


(A.  22a) 
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or 


< 


Sr  =  SV  +  t>E“I  X  (sv  +~&E~l  X  Sr)  . 

Repeating  the  same  process  with  Eq.  (A. 16)  yields, 

V+5V=?  +  s"f  +  <3+5(3-  _SE"'1  X  X  (v  +  Sr^J 

-(wE“I  +U?c"1)  X  (v  n  SV  ) 

Subtracting  Eq.  (A. 16)  from  Eq.  (A. 23)  yields 

SV  =  &£  +  5G  -  a?5*'1  X^2'1  X  Sr) 

-^7>E‘I  +  u?C"1)  x  SV 

Again,  the  Coriolis  Law  is 

i  _,c-I 

S  V  =  SV  +  u  X  SV 


Substituting  Eq.  (A. 24)  into  Eq.  (A. 25)  gives 

&vN  8  f  +  5G  -u^-1  X^-1  X  S?)-^2"1  X  SV 

Now  derive  the  terms  in  &G,  which  is  a  function  of  r  and  of 
time  t. 


— »  -4  Vr?  -4—4  -4  “4 

G  =  G(r,  t)  = - j  +  e(r>  fc)  +  ’l(r> 

r 


where 


e(r,  t)  A  oblateness  of  earth  and  sun-moon  effects 
i\(X,  t)  A  randomness  effects. 


.22b) 


.23) 


(A. 24) 


(A. 25) 


(A. 26) 


(A. 27) 
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Thus, 


t  +  6G  =  +  t  +  5?  +  ? 

[(r  +  &r)  .  (r  +  &r)J  ^ 


(A.  28a) 


-  r&K 
3 


K(r  h  6r) 


(t2  +  &r2  +  2r  .  6“r)2 


-s  +‘e+&e>  +  Tf  .  (A. 28b) 


&r  =  r  x  8  r  | 

i.e.,  5r  is  a  scalar  and  r  is  a  unit  vector  in  the  r  direction  given  by 

*  "r 
r  = 

M 

Consequently,  performing  a  binomial  expansion  on  the  denominator  and 
neglecting  higher  order  terms  yields 


-»  -»  -r^8K  K  i.  $r  .  hr  \ 

G  +  5G  —  j  ”  3  (r  +  8t)  fl  “  '  2  I 


+  e  +  &e  +  i)  .  (A. 29) 


r  r 


Subtracting  Eq.  (A. 27)  fromEq.  (A, 29), 


r  r  r 


(A.  30) 


Now  defining 


2  A  K 

us  =  ~3 
r 


to  be  a  constant  when  r  «  ^earth’  .anc*  n'ore  Prec*se^y 


o6  »  •  or  , 
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Appendix  B 

AN  OVERVIEW  OF  RADAR  AREA  CORRELATION  NAVIGATION 

B.l  GENERAL 

This  is  a  short  compendium  abstracted  primarily  from  the  following 
authors:  Wiley  [18],  Stauffer  [19],  Eppler  and  Willstadter  [25], 
Develet  [26],  and  Diamantides  [27].  It  is  not  intended  to  be  a  trea¬ 
tise  on  the  subject  but  rather  a  convenient,  self-contained  reference 
on  radars  in  the  navigation  application  as  compared  to  radar 
transmitter- receiver  design. 

It  is  possible  to  use  an  airborne  radar  to  obtain  completely  auto¬ 
matic  navigation  by  comparison  of  the  image  generated  by  the  vehicle's 
radar  in  flight  with  a  series  of  stored  reference  radar  images  made  by 
previous  reconnaissance.  The  comparison  is  made  by  finding  the  auto¬ 
correlation  or  cross  correlation  between  the  live  image  and  the 
reference  image;  this  process  is  called  radar  area  correlation  or 
simply  radar  nap  matching. 

The  radar  map-matching  process  determines  the  displacement  of  the 
live  radar  image  with  respect  to  the  position  of  the  reference  image. 

By  "position"  of  an  image  is  meant  the  geographical  location  of  the 
radar  that  will  create  the  image  in  question.  The  displacement  data, 
in  geographic  coordinates,  is  then  fed  back  to  the  navigation  computer 
to  update  computed  position.  Figure  B.l  shows  the  geometry  of  the 
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FIG.  B.l.  TYPICAL  PROBLEM  GEOMETRY 

problem.  Because  the  map  matcher  can  deliver  almost  continuous  error 
data,  it  is  conceivable  that  these  signals  could  be  used  to  steer  the 
vehicle  directly.  However,  the  area  correlation  data,  although  free  . 
of  error  that  accumulates  with  time,  contain  high-frequency  noise 
resulting  from  the  continuous  fluctuations  in  the  radar  image. 

Attempts  to  smooth  out  this  noise  by  integration  result  in  area  corre¬ 
lation  time  constants  that  are  too  long  to  control  the  vehicle  in  a 
stable  manner.  The  solution  is  one  of  combining  a  fast-responding, 
low-noise  device  that  has  the  disadvantage  of  accumulating  errors 
causing  long-term  drift  (such  as  an  inertial  platform)  with  a  slower, 
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noisy  device  without  drift,  the  area  correlator.  By  proper  allotment 
of  time  constants  between  the  two  devices,  it  is  possible  to  produce 
a  fast- responding  system  with  little  noise  and  no  drift. 

B.2  PRINCIPLES 

The  basic  principle  of  an  active  radar  ground  mapping  system  is 
that  it  transmits  energy  and  detects  the  part  of  it  scattered  back 
from  a  target.  However,  instead  of  the  usual  point  target,  the  target 
in  this  case  is  the  ground,  which  can  be  considered  as  an  extended 
array  of  scatterers.  The  radar  map  is  obtained  by  scanning  or  painting 
the  ground  and  displaying  the  return  on  a  cathode  ray  tube  or  photo¬ 
graphic  film.  Because  the  scattering  characteristics  of  the  ground 
will  vary  from  point  to  point,  the  nap  will  be  in  the  form  of  a  varying 
brightness  pattern.  Variations  of  intensity  in  this  brightness  pattern 
can  be  interpreted  in  terms  of  the  topographical  and  man-made  features 
of  the  terrain.  For  example,  the  energy  back- scattered  from  a  smooth 
surface  such  as  calm  water  will  be  much  less  than  that  from  a  rough 
surface  such  as  the  ground.  The  degree  of  correspondence  between  the 
brightness  pattern  and  the  features  of  the  terrain  depends  on  the 
characteristics  of  the  antenna  beam  pattern  used  to  paint  the  ground. 

The  antenna  beam  pattern  usually  employed  in  ground  mapping  sys¬ 
tems  is  narrow  in  one  dimension  and  has  wide  angular  coverage  in  the 
other  dimension.  This  type  of  beam  is  known  as  a  fan  beam.  It  is 
usually  oriented  so  that  the  narrow  dimension  is  horizontal,  thus 
illuminating  a  long  narrow  strip  of  ground  from  beneath  the  vehicle  to 
some  maximum  range.  Thus,  for  a  given  pointing  direction,  the  radar 
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beam  illuminates  targets  at  many  different  ranges  and  depression 
angles.  Variations  in  the  brightness  are  therefore  functions  of  range 
and  the  angle  at  which  the  ground  is  viewed  as  well  as  the  reflective 
properties  of  the  terrain.  This  condition,  if  not  corrected,  would 
complicate  the  correlation  between  the  radar  image  and  terrain.  To 
compensate  for  the  effects  of  range  and  viewing  angle,  the  vertical 
gain  pattern  of  a  radar  ground  mapping  antenna  is  designed  to  be  a 
function  of  the  depression  angle  at  which  a  given  patch  of  ground  is 
viewed.  This  type  of  pattern  is  as  a  cosecant-squared  beam. 

Scanning  of  the  antenna  beam  is  usually  accomplished  either  by 
rotating  the  antenna  about  a  vertical  axis  or  by  positioning  the 
antenna  along  the  vehicle  so  that  its  motion  provides  the  scanning. 
When  the  beam  is  rotated  a  full  360  degrees  about  the  vertical  axis, 
the  image  is  usually  in  the  form  of  a  plan  position  map  with  the 
vehicle  at  the  center.  Most  systems  employing  this  type  of  scan  use 
a  sector  scan,  i.e.,  less  than  the  full  360  degrees.  When  the  sector 
is  directed  forward  of  the  vehicle,  the  system  is  known  as  a  forward- 
looking  area  correlation  radar.  Systems  employing  the  velocity 
scanning  technique,  where  the  beam  is  directed  to  the  side  of  the 
vehicle,  are  known  as  side-looking  radars.  The  images  obtained  with 
this  system  are  in  the  form  of  strip  maps  along  each  side  of  the 
vehicle's  track  and  are  especially  adapted  to  the  use  of  photographic 
techniques  to  obtain  a  permanent  record.  This  permanent  record  or 
signal  storage  has  led  to  the  consideration  of  the  correlation  process 
as  an  integral  part  of  radar  image  matching. 
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B.3  RADAR  IMAGE  MATCHING  -  CORRELATION 


/ 

I 

j 

Imagery  matching  is,  in  essence,  a  way  of  measuring  the  similarity 
between  two  displays;  its  outcome  conveys  information  not  only  about 
the  displays'  structures  but  also  about  their  relative  positions.  The 
former  is  of  value  to  the  problem  of  quantifying  displays  of  scenes  or 
objects  anu,  subsequently,  to  the  problem  of  attaching  meaningful 
characteristic  numbers  to  the  scenes  or  objects  in  question  for  the 
purpose  of  classification.  The  latter  lends  itself  to  position  fixing 
and  therefore,  if  the  displays  are  maps,  to  automatic  navigation  and 
homing  guidance. 

The  study  of  display  matching  is  essentially  a  study  of  the  corre¬ 
lation  function  and  inasmuch  as  a  figure,  map,  or  other  subject  is 
displayed  in  two  dimensions.  The  correlation  function  under  consider¬ 
ation  is  for  two  dimensions, 

Y  X 

T])  =  f  J  T(x,  y)  T (X  +  I ,  y  +  q)dxdy  ,  (B.l) 

-Y  -X 

where  T(x,  y)  is  the  display  brightness  at  (x,  y)  and  |,  tj  the  relative 
displacements . 

In  display  matching,  there  is  generally  a  current  image  which  is 

compared  to  a  memory  image,  the  output  of  this  comparison  being  the 

input  to  a  detection  system.  If  the  two  images  are  different,  the 

resultant  output  is  mutual  property  of  both  image  functions  and  can  be 

/ 

ascribed  no  more  to  one  of  the  images  than  to  the  other.  The  process, 
then,  is  called  cross  correlation.  If,  however,  an  image  is  correlated 
with  a  duplicate  of  itself,  the  output  is  wholly  a  property  of  that 
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image  and  the  process  is  called  autocorrelation.  Where  cross  correla¬ 
tion  is  executed  for  functional  purposes,  autocorrelation  is  performed 
for  purely  analytic  reasons.  According  to  one  school  of  thought,  the 
pattern  recognition  process  that  occurs  when  a  pilot  observes  a  target 
scene  or  when  a  photo  interpreter  examines  an  aerial  photograph  is 
similar  to  the  correlation  process  previously  described.  The  principal 
difference  between  the  two  is  that  memory  and  detection  are  contained 
within  the  physiological  equipment  of  a  human  being. 

In  some  early  mechanizations,  area  correlation  was  done  by  projec¬ 
ting  the  live  radar  image  onto  the  stored  reference  image  and  measuring 
the  total  light  emerging  from  the  back  of  the  reference  image  (Fig. 
B.2).  If  the  two  signals  are  statistically  alike,  <t>(|,  tj)  is  largest 
when  the  images  are  in  register  and  §  =  tj  =  0.  As  the  images  move  out 
of  register,  so  that  |  and  tj  are  not  zero,  <t>(g,  tj)  becomes  smaller, 
decreasing  asymptotically  to  zero  for  very  large  displacements, 
completely  destroying  any  statistical  similarity  between  the  image 
elements  in  the  product. 

If  one  image  is  rapidly  scanned  over  the  other  in  a  small  circle, 
the  output  light  will  fluctuate.  The  major  component  of  this  fluctua¬ 
tion  will  be  at  the  nutation  rate  because,  in  the  general  case,  the 
nutation  circle  will  be  closest  to  the  correlation  peak  at  one  point  in 
the  circle  and  furthest  away  (180  degrees)  in  nutation  phase  from  the 
instant  of  maximum  light.  The  phase  of  the  fluctuation  will  change 
with  respect  to  the  nutation  drive  if  the  nutation  circle  moves  around 
the  correlation  peak  at  g,  tj  «  0.  One  can  synchronously  demodulate  the 
light  fluctuation  with  respect  to  the  in-phase  and  quadrature  nutation 
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FIG  B.2.  IMAGE  MATCHING  MECHANIZATION 


drive  signals  and  integrate  the  demodulator  outputs.  The  integrator 
outputs  are  servo-error  signals,  changing  in  algebraic  sign  at  g,  Tj  =  0 
in  the  correct  manner  to  register  the  images,  when  they  are  applied  to 
servoamplifiers,  which  move  one  of  the  images  with  respect  to  the 
other.  In  this  manner,  displacement  of  one  image  with  respect  to  the 
other  can  be  measured.  If  the  radar- image  signals  were  identical,  and 
if  the  correlator  could  operate  on  infinitely  large  samples  of  the  two 
signals,  then  the  cross  correlation  function  $  (g,  t|)  would  be  a 
noiseless  smoothly  varying  function  of  g  and  tj  and  would  have  a  per¬ 
fectly  defined  maximum.  The  correlation  tracker  could  then  determine 
the  correlation  peak  position  and,  hence,  the  register  point,  to  any 
desired  accuracy.  However,  the  signals  are  not  identical  and  the 
correlator  must  work  with  finite  samples  of  the  image  signals.  In 
general,  the  two  images  are  not  made  at  the  same  location;  as  a  result, 
scintillation,  moving  shadows,  and  the  like  destroy  identicality.  In 
addition,  there  is  receiver  noise  in  both  images.  Even  if  the  signals 

were  identical,  the  finite  signal  sample  size  would  produce 
fluctuations  from  correlation  to  correlation. 
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From  both  of  the  previously  mentioned  cases,  then,  the  value  of 
the  cross  correlation  function  generated  for  a  given  g,  tj  varies 
unpredictably  from  one  correlation  to  the  next,  as  does  the  location 
of  the  maximum  value.  Because  the  location  ot  the  maximum  value 
(g  =  ij  =  0)  is  the  output  quantity  of  interest,  its  variation  from 
correlation  to  correlation  is  a  fluctuating  error  in  the  displacement 
measurement,  which  determines  the  accuracy  of  the  map-matching  process. 
As  a  rule  of  thumb,  experience  has  shown  that  a  well-designed  system 
can  measure  g  and  rj  to  approximately  one-half  the  radar-range  resolu¬ 
tion  if  areas  ahead  and  to  the  side  of  the  vehicle  are  simultaneously 
used  in  the  match  process.  This  placement  of  the  matched  areas  insures 
reltative  motion  of  the  images  in  the  range  direction,  which  is  the 
narrow  dimension  of  the  target  elements  for  changes  of  g  and  tj. 

B.4  SYSTEM'S  CONSIDERATIONS 

Some  of  the  most  important  system  considerations  for  ground  image 
referencing  radar  systems  involve  resolution,  accuracy,  range  and 
operational  altitude,  and  all-weather  capability. 

As  accuracy  is  the  basic  measure  of  navigation  systems  per¬ 
formance,  resolution  is  the  basic  measure  of  performance  for  radar 
ground  area  correlation  systems.  Resolution,  a  measure  of  the  system's 
ability  to  distinguish  between  closely  spaced  objects  or  to  u^lineate 

the  details  of  a  large  area,  is  usually  defined  in  terms  of  range 
resolution  and  transverse  or  azimuth  resolution.  While  the  ultimate 
resolution  attained  by  the  system  is  a  function  of  many  parameters,  the 
single  criterion  most  commonly  used  to  judge  it  is  the  pulse  packet 
size  as  projected  on  the  ground.  The  system  parameters  which  determine 


116 


the  pulse  packet  size  are  antenna  beamwidth  and  pulse  length,  as 
measured  at  the  half-power  points. 

Beamwidth  is  a  function  of  the  transmitted  wavelength  and  the 
dimensions  of  the  antenna  being  expressed  approximately  by 

e  *  y  >  (b.2) 

where 

d  =  beamwidth  defined  by  the  half-power  points 

\  =  transmitted  wavelength 

D  =  pertinent  dimension  of  the  antenna  aperture 

K  =  constant  dependent  on  the  particular  aperture.  A  typical 

value  for  this  constant  is  70  where  Q  is  expressed  in  degrees 
and  X  and  D  are  measured  in  the  same  units. 

The  system  designer  is  confronted  at  once  with  a  compromise  in  the 
selection  of  the  transmitted  wavelength  and  the  dimensions  of  the 
antenna.  To  narrow  the  beamwidth,  either  the  wavelength  must  be 
decreased  or  the  dimensions  of  the  antenna  must  be  increased.  In 
decreasing  the  wavelength,  the  problems  of  atmospheric  attenuation  and 
the  generation  of  large  amounts  of  power  become  increasingly  difficult. 
The  maximum  size  of  the  antenna  obviously  will  be  limited  for  airborne 
installations . 

Range  resolution  may  be  improved  by  decreasing  the  pulse  length. 
Again,  there  is  a  minimum  limit  because  the  average  power  trans¬ 
mitted  is  a  direct  function  of  pulse  length.  Also  to  be  considered  in 
this  connection  is  the  altitude  at  which  the  system  will  operate 
because  the  length  of  the  pulse,  as  projected  on  the  ground,  is  a 
function  of  the  radar  altitude. 
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Another  important  area  with  regard  to  system  resolution  concerns 
the  receiver.  Here,  one  of  the  basic  considerations  is  its  dynamic 
range,  i.e.,  the  range  of  signal'  amplitudes  which  it  can  accommodate 
without  distortion.  Because  a  radar  image  is  a  brightness  pattern, 
variations  of  intensity  within  the  image  contribute  to  the  resolution 
of  certain  features.  The  range  of  signal  amplitudes  encountered  in  a 
given  correlation  operation  may  be  large.  If  the  receiver  or  display 
system  cannot  accommodate  such  a  range  of  signal  amplitudes  without 
distortion,  loss  of  much  detail  within  the  image  will  occur.  If  the 
gain  or  intensity  is  set  at  the  noise  threshold,  strong  targets  will 
"bloom",  thus  obscuring  nearby  weaker  targets.  If  the  gain  or  inten¬ 
sity  is  set  too  high,  the  weaker  targets  will  not  be  mapped. 

System  accuracy  is  as  equally  important  in  many  respects  as  system 
resolution.  Distorted  or  "smeared"  images  make  it  difficult  to  obtain 
a  true  measure  of  ground  distances  or  to  resolve  details  within  the 
image.  Types  of  errors  that  can  occur  are  altitude  errors,  drift 
errors,  and  stabilization  errors.  Errors  in  the  measurement  of  the 
altitude  of  the  mapping  vehicle  can  cause  distortions  in  the  image 
because  in  most  systems  altitude  is  used  to  set  in  the  range  scan 
factor.  Angular  distortion  of  the  image  can  occur  also  if  there  is  no 
compensation  made  for  drift. 

Stabilization  errors  occur  when  the  antenna  is  not  stabilized  for 
pitch  and  roll  displacements.  Such  errors  produce  distortion  and 
smearing  of  the  image.  The  degree  of  distortion  or  smearing  that  will 
occur  is  difficult  to  define  analytically.  However,  related  studies  on 


airplane  motions  in  turbulent  air  have  indicated  that  displacements  of 
as  little  as  1  degree  in  pitch  occur  with  such  frequency  as  to  make 
stabilization  desirable . 

B.5  SIGNAL  PROCESSING  AND  A  MECHANIZATION  SCHEME 

The  procedure  starts  with  the  acquisition  of  a  reference  image  by 
e.g.,  a  recon^i'ssance  satellite.  The  image  correlation  process  is 
concerned  with  determining  the  position  offset,  with  respect  to  this 
stored  reference  image,  of  terrain  whose  radar  return  is  scanned  by  the 
reentry  body.  To  process  the  data  digitally,  the  radar  and  reference 
images  must  be  sampled  in  space  and  quantized  in  intensity. 

By  space  sampling,  it  is  meant  that  the  radar  signal  is  sampled  at 
particular  instants  of  time  and  these  samples  are  used  to  represent 
areas  on  the  ground.  The  size  of  these  areas  depends  on  the  radar 
resolution  and  the  signal  processing.  Intensity  quantization  means 
that  the  amplitude  of  the  radar  signal  at  the  sampling  instant  is  rep¬ 
resented  by  one  of  K  discrete  values.  For  the  binary  case,  (K  **  2),  a 
sample  is  stored  as  a  "1"  or  "O”  depending  on  whether  or  not  its 
amplitude  exceeds  a  specified  threshold. 

As  a  result  of  space-sampling  and  amplitude  quantization,  the 
radar  and  reference  images  can  be  represented  as  a  matrix  in  which 
each  element  represents  the  radar  return  from  a  particular  area  of  the 
image.  In  the  binary  case,  the  condition  for  correlation  of  an  element 
of  the  reference  image  with  an  element  of  the  radar  image  is  that 
both  elements  are  alike,  i.e.,  both  have  the  value  "1"  or  both  have  the 
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value  "0".  The  image  area  correlation  system  must  compare  reference 
and  radar  images  element  by  element  and  count  the  number  of  places  in 
which  they  agree. 

In  the  correlation  process,  the  radar  image  is  in  effect  super¬ 
imposed  in  each  of  a  matrix  of  discrete  positions  over  the  reference 
image  and  a  measure  of  the  resultant  correlation  is  obtained  for  each 

superposition.  Provided  it  exceeds  a  predetermined  correlation  thres- 

* 

hold  level,  the  sampled  superposition  yielding  maximum  correlation  is 
taken  as  the  best  estimate  for  the  true  superposition  region.  The 
precise  best  estimate  of  position  within  this  region  is  then  made  by 
employing  interpolation  techniques. 

To  be  able  to  locate  the  match  point  with  an  error  less  than  one- 
half  the  distance  between  samples,  it  is  necessary  to  use  an  interpola¬ 
tion  procedure  .  If  this  were  not  done,  it  would  be  necessary  to 
decrease  the  distance  between  samples,  thus  requiring  higher  radar 
resolution,  larger  computer  memory,  and  longer  computation  time. 

Thus,  the  area  correlation  system  must  perform  four  operations: 

a)  Store  the  space-sampled,  amplitude  quantized  radar  signal  as 
a  matrix  representation  of  the  mapped  area. 

b)  Shift  the  radar  and  prestored  reference  images  relative  to 
each  other,  conceptually  as  shown  in  Fig,  B.2. 

c)  Determine  the  correlation  between  radar  and  reference  images 
for  each  possible  offset.  For  the  two-level  system,  for  example,  the 
correlation  is  obtained  by  counting  the  number  of  elements  in  which  the 
radar  and  offset  reference  matrices  coincide. 

d)  Interpolate  between  offsets  to  determine  the  match  point. 
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Appendix  C 

OPTIMAL  ESTIMATION  OF  STATES  IN  A  LINEAR  SYSTEM 
WITH  CORRELATED  PROCESS  NOISE  AND  MEASUREMENT  NOISE 

C.l  GENERAL 

The  idea  is  to  estimate  the  state  of  a  system  x(t)  from  observed 
or  measured  data  z(t)  where  x  and  z  are  vector  quantities.  There  is  a 
known  relationship  between  the  observation  and  state  vectors  and  there 
is  additive  noise  present  in  the  observation.  These  comments  can  be 
expressed  in  the  continuous  case  as 

i 

x(t)  *  F(t)  x(t)  +  G(t)  u(t)  (C.i) 

and 

z(t)  =  H(t)  x(t)  +  v(t)  ,  (c  .2) 

where 

x(t)  £  n  x  1  vector  of  state  variables 
u(t)  A  n  x  1  vector  of  input  noise 

s a 

F(t)  A  n  x  n  matrix  representing  linear  dynamics 

S3 

G(t)  A  m  x  n  matrix  representing  the  effect  of  the  input  on 
dynamics 

z(t)  A  p  x  1  vector  of  system  outputs  (observations) 

H(t)  A  p  X  n  matrix  relating  x  and  z 

V(t)  A  P  X  1  vector  of  noise  in  the  measurement. 

Furthermore,  u(t)  and  V(t)  are  Gaussian  white  noise  random  variables 
with  zero  mean  and  auto-covariance  matrices 
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The  symbol  E[  }  denotes  the  expected  value  of  the  quantity  in  the 
T 

brackets,  A  denotes  the  transpose  of  matrix  A,  and  the  quantity 
8(t  -  -r)  is  the  Dirac  5-function. 

C.2  PROBLEM  AND  PURPOSE 

The  problem  is  to  derive  the  main  restilts  of  the  Kalman-Bucy 
filter  [8]  with  the  extension,  not  originally  considered  in  their  paper, 
that  correlated  input  and  process  measurement  noise  is  to  be  expected. 
The-  purpose  is  to  develop  familiarity  with  the  techniques  of  their 
classic  paper,  obtain  useful  results  for  further  applications,  and  have 
a  basis  for  discussion  of  their  work  as  a  self-contained  item  in  this 
report , 

0.3  SOLUTION 

With  no  pretense  of  originality,  the  solution  begins  with  the 
results  given  in  the  paper  by  Kalman  and  Bucy  [8]  as  Eq.  (38).  Their 
Eq.  (38)  states,  in  essence,  that  the  Weiner-Hopf  equation  yields  a 
necessary  and  sufficient  condition  for  a  minimum  variance  estimator  of 

fx(t),  x^)]  > 
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i.e.. 


cov 


jjc^t^,  z(0)J  *  A^tj,  Tj  cov  jz(T),z(0)Jdt , 


V  O<0<t  .  (c .6) 


Continuing  the  development,  let  t^  =  t  for  simplicity  in  the  development 
and  differentiate  the  left  side  of  Eq.  (C.6)  with  0  <0  <  t. 

Then, 

cov[x(t),  z(o)]  =  cov^x(t),  z(o)J  +  cov|x(t),  z(n)J 
=  cov jV(t)  x(t)  +  G(t)  u(t),  z(o>] 

=  cov[F(t)  x(t),  z(o)]  +  cov^G(t)  u(t) ,  z(a)J 
=  F(t)  cov|x(t),  z(a)]  +  G(t)  cov^u(t) ,  H(o)  x(a) +v(o)J 
=  F(t)  covjx(t),  z(a)]  +G(t)  H(o)  covju(t),  x(a)J 
+  G(t)|cov  u(t),  v(a)]  .  (C-7) 

Now  differentiating  the  right  side  of  Eq.  (C.6)  using  Liebnitz's  rule. 


4/ 


A(t,  t)  cov[z(t),  z<0)]dT  =  -  0+  A(t,  t)  cov[z(t),  z(o)l 


1 


+  |  ^  A(t,  t)  c ov [  z  (0  ) ,  z(T)]dT  , 


(C.8) 


where 


cov[z(t),  z (0 ) ]  =  cov[H(t)  x(t)  +v(t),  Z(0>] 


=  H(t)  cov[x(t),  z(0>]  +cov[v(t),  z(0>] ,  (C.9) 


and 


t 

/ 


H(t)  cov[x(t),  z(o)]  =  H(t)  /  A(t,t)  cov[z(x),  z<0>]  dr  .  (C.10) 

(C-6) 

0 
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From  Eqs.  (C.6),  (C.7),  and  (C.8) 


r 


_d_ 

dt 


■1 


cov[x(t),  z(ct)]  =  /  A(t,  t)  covfz(n),  z(r)]dT 


F(t)  cov[x(t),  z (a)]  +  G(t)  H(a)  cov[u(t)  x(a)]  +  G(t)  cov[u(t),  v(a)] 
=  A(t,  t)  H(t)  cov[x(t),  z(o)]  +  A(t,  t)  cov[v(t) ,  z(a)] 

+  /  5t  A(t>  cov£z(a>>  8(x)]dT,  ¥■  0 <a <  t  .  (C.ll) 

Noting  also  in  Eq.  (C.ll)  that 

A(t,  t)  covfv(t),  z (cr) ]  =  A(t,  t)  covfv(t),  H(a)  x(a)  +  v(o)] 

=  A(t,  t)  H(a)  cov[ v (t) ,  x(o)]  +A(t,  t) 

cov[v(t),  v(a)]  ,  (C .12) 

and  putting  it  in  the  integral  form  of  Eq.  (C.6)  where  appropriate, 

Eq.  (C.ll)  becomes  V  0  <  a  <  t. 


[  A(t  ,i) 


F(t)  ^  A(t,  i  )  cov[ z(o) ,  Z (t  ) |  dx  -  A(t,  t)  H(t)  /  A(t,  T  )  covf  z(a)  ,  z  (x  )1  dT 


I 


■  t  A(t,  i )  covfs(a),  2  (~  )  I  di  +  C(C)  H(a)  cov[u(t),  x(o)l  +  G(t)  covfu(t),  v(t)J 


-  A(t,  t)  H(a)  cov[v(t),  x(rr)l  +A(t,  t)  covfv(t),  v(a)  ]  «  0  .  (C.13) 

The  last  four  terms  in  Eq.  (C.13)  can  be  rationalized  to  zero  as 
follows: 
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m  rni  ii*ii4 


G(t)  H(ff)  cov  [u(t),  x(o)]  =  0  because  present  noise  measurement  is 

uncorrelated  with  previous  condition 
of  state 


G(t)  cov  [u(t),  v(cj)]  =  0 


because  present  noise  measurement  is 
uncorrelated  with  previous  condition 
of  state 


H(o)  cov[v(jt),  x(a)]  =  0 
cov[v(t),  v(o)]  =  0 


Thus  Eq.  (C.13)  becomes, 


for  the  time  a  <  t 

because  past  noise  does  not  influence 
present  noise  measurement. 


J  jf“(t)  A(t,x)  -  A(t,t)  H(t)  A(t,  t) (t,  x)J  cov|z (cj) ,  z (x)j  dr  =  0  .(C.14) 
This  is  satisfied  if  A(t,  x)  is  a  solution  of  the  equation.  Therefore, 


(t,  t)  =  F(t)  A(t,  t)  -  A(t,  t)  H(t)  A(t,  x) 


(C .15) 


Deriving  a  differential  equation  for  x(t)  commences  with 


x(t|t) 


-  t  Mt. 

Jr\  • 


t)  z(t)  dx 


(C .16) 


Thus, 


x(t|t)s  -rrl  A(t,  x)  z(t)  dx  =  A(t,  t)  z(t)  + 


/  dA(t,  xl 

J  ^ 

•'A 


z(x)  dx  .  ,(C  .17) 


Putting  Eq.  (C.15)  into  Eq.  (C.17)  gives 


x(t }  t)  a  A(t,  t)  z(t)  +  /( F(t)  A(t,  t)  -  A(t,  t)  H(t)  A(t,  x)  z(r)dx  (C.18) 


=  f F (t )  -  A(t,  t)H(t)] 


/A(t,  x 


)  z(x)  dx  +  A(t,  t)  z(t) 


(C  .19) 
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or  finally, 


x(t|t)  =  F (t)  x(t|t)  +  A(t,  t)  [ z (t)  -  H(t)  x(t|t)]  ,  (C.20) 


V  0  <  a  <  t 


Now  solve  for  the  optimum  gain,  noting  that  A(t,  t)  A  K(t) ,  by  using 
Eq.  (C.6)  again, 


H'i)  ,  z(n)J  =  J  &(ty  t)  cov[z(t),  z(a)]  dr 


,  ¥  0  <  a  <  t  . 

(C  .21)) 


Rewrite  by  letting  =  t  to  obtain 


cov[x(t),  y(o)  + 


v (cr ) ]  =  /  A(t,  t)  cov[y(T)  +v(t),  y(o)  +  v(o)]dT 

T>  (C  .22) 


Expanding  the  left  side  of  Eq.  (C.22)  yields 


r[x(t),  y(a)  +  v(cr)]  =  cov[x(t),  y (a))  +  cov[x(t),  vT(o)] 

=  cov[x(t),  xT(a)  HT(a)]  +cov[x(t),  vT(o)] 

=  cov|x(t),  xT(o)]hT(ct) +cov[x(t),  vT(cr)|.  (C.23) 


At  time  a  »  t, 


cov[x<t),  y(t)  +  v(t)J  =  cov[x(t),  xT(t)]HT(t)  +  cov[x(t),  vT(t)j  . 


(C.24) 


X(t)  =  $>(t,  0)  X(0)  + 


f<H  t, 

*7\ 


t)  G(t)  u(t)  dx  >  (C.25) 


therefore, 


t 

r[x(t),  vT(t)]  =  /  $(t,  -r)  G(t)  cov[u(t),  vT(t)]  dT  ,  (C.26) 


because  x(0)  is  independent  of  v(t),  t  £  0  . 

But, 

cov[u(t),  vT(t)j  =  S (t)  S(t  -  t) 

so  that  Eq.  (C.26)  is 

covjx(t),  vT(t)J  =  G(t)  S(t)  , 

and  finally  the  left  side  of  Eq.  (C.21)  becomes 

cov^x(t),  zT(t)j  =  covjx(t),  xT(t)j  HT(t) +G(t)  S(t) 

Now  under  the  integral  sign  in  Eq.  (C.21), 

cov[z(t),  zT(o)j=  cov[z  (t)>  zT(t)j  at  o'  =  t 
Therefore, 

cov[z(t)  zT(o)]  =  cov[z(t),  H(t)  x(t)  +  v(t)T] 


(C .27) 


(C .28) 


(C .29) 


(C  .30) 


cov 

z(t), 

H(t)  X 

(t)  +  V(t)T] 

(C.31) 

cov| 

[z(t), 

xT(t)] 

H^(t)  +  cov 

[z(t), 

vT(t)] 

cov| 

z(t), 

rt 

H^(t)  +  cov 

[h(t) 

x(t)  +  V(T),VT(t)] 

cov| 

Z(t), 

xT(t)] 

HT(t)  +  H(t) 

cov  [ 

x(t),  VT(t)] 

+  cov 

[v(t). 

rt 

,  t 

cov 

[z(t). 

>  xT(t) 

]  HT(t)  +  H(t)  cov 

[x(t),  vT(t)] 

+  R(t)  &(t  -  t). 


(C .32) 


Putting  Eqs.  (C.29)  and  (C.30)  into  Eq.  (C.21)  gives 


tL 

r[x(t),  xT(t)]HT(t)  +  G(t)  S(t)  -  /  A(t,  t)  cov [z (t ) »  xT(t)]HT(t)  dT 


-  f  A(t, 


t)  H(t)  cov  x(t) 


,  vT(t)j  dT  -  f 


A(t,  T)  R(t)  8(t  -  t)  dT  =  0 

(C  .33) 


K. 

I  A(t,  t)  H(t )  covjx(x),  vT(t)jdr  =  0  , 


(C .34) 


because 


r[x(x),  vT(t)]  = 


G(t)S(t),  t-r 


Except  for  the  infinitesimal  instant  when  t  =  t,  the  integral  in 
Eq.  (C.34)  is  zero.  Also, 


la, 


t)  R(t)  6 (t  -  x )dr  =  A(t ,  t)  R(t)  A  K(t)  R(t)  . 


Therefore  Eq.  (C.33)  becomes 


t 

cov[x(t),  xT(t)]  HT(t)  +  G(t)  S(t)  -  I  A(t,  t)  cov^z(f),  xT(t)j  HT(t)  dr 


-  K(t)  R(t)  =  0 


(C  .35) 
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Now  appealing  to  mathematical  formalities  of  continuity,  differen¬ 
tially,  etc.,  which  are  necessary  to  interchange  the  integral  and 
covariance  operations, 

HT(t)  +  G(t)  S(t)  =  K(t)  R(t) , 

(C .36) 


cov 


x(t) 


■1 


A(t,  t)  z(r)dx, 


xT(t) 


but  from  Eq.  (C.16) 


A(t,  t)  z  (t)  dr  =  x(t  |t)  A  x(t) 


Therefore,  Eq.  (C.36)  becomes 


[* 


covlx(t)  -  x(t),  X 


'(t)]  H'1 


(t)  +  G(t)  S(t)  =  K(t)  R(t) 


(C  .37) 


/ 


or 


cov 


HT(t)  +  G(t)  S(t)  *  K(t)  R(t) 


(C .  38) 


Now, 


cov[x(t),  xT(t)]  =  cov[x(t),  xT(t)  +  xT(t)] 

a  cov[x(t),  xT(t)]  -J-  cov[x(t),  £T(t)] 

*  cov[x(t),  xT(t)]  a  Yxt)  ,  (C . 39) 

where  the  last  covariance  term  is  zero  because  the  error  is  perpendicu¬ 
lar  to  its  estimate  [7].  Now,  putting  Eq.  (C.39)  into  Eq.  (C.38), 


> 
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E(t)  H  (t)  +  G(t)  S(t)  =  K(t)  R(t)  , 
so  that  the  optimal  gain  is 

K(t)  =  [Z(t)  H(t)  +  G(t)  S(t)  R(t)-1]  . 

For  the  covariance  Riccati  equation  it  is  noted 
Z(t)  =  cov£x(t) ,  x(t)j  , 

7(t)  =  cov[x(t),  x(t)j  +  cov[x(t),  x(t)j 

Defining  ■ 

x(t)  4  x(t)  -  x(t) 

x(t)  4  x(t)  -  x(t)  , 

then, 

x(t)  =  F(t)  x(t)  +  G(t)  u(t)  -  F(t)  x(t)  -  K(t)[z(t)  -  H(t)  x(t)] 

=  F(t)  x(t)  +  G(t)  u(t)  -  F(t)  x(t)  -  K(t)[H(t)  x(t)  +  v(t)  - 

=  F(t)  Jx(t)  -x(t)J-  K(t)  H(t) |x(t)  -  x(t)J  +  G(fc)  G(t)  ”  K(t)  v(t) 

Thus, 

x(t)  =  (F(t)  -  K(t)  H(t)]  x(t)  +  G(t)  u(t)  -  R(t)  v(t) 
The  solution  to  this  differential  equation  is 

x(t)  =  <!>(t,  0)  x(0)  +  /$( t,  t){g(t)  u(t )  -  K(t)  v(t)]dT 

J0 


(C  .40) 


Next,  put  Eq.  (C.46)  and  (C .48)  into  Eq.  (C.43)  noting  again, 


cov[u(t),  uT(t)]  =  Q(t)  6 (t  -  t) 
cov[v(t),  vT(t)]  =  R(t)  5 (t  -  t) 
cov[u(t),  vT(t)]  =  S(t)  &(t  -  t)  . 

To  simplify  the  notation,  drop  the  arguments  henceforth 

Z  =  cov[(F  -  KH)x  +  Gu  -  Kv,  x]  +  cov[x,  (F  -  KH)x  +  Gu  -  KvJ 

=  F  cov[x,  x]  -  KH  covjx,  x]  +  G  cov[u,  x j  -  K  cov|v,  x] 

T  r~  T  t  nv,  t  t  r~-’T 
x,  xjF  -  covLx,  xjH  K  +  cov]_x,  ujG  -  covLx,  v.K 


(C  .48) 


Z  ^  FT  -  KHZ  +  |  gqgt  -  krk  -  Z  ft  -  Z  htkt  +|  gqgt  -  gskt  -  kst  gt  . 

(C .49) 

i  =  fZ  +  ZFT  -  k[z.HT+Gs]T  -  [in1  +Gs]kT  +  KRKT  +  GQGT  .  (C, 50) 

Using  Eq.  (C .41)  for  K  in  Eq.  (C.50), 


Z  =  fZ  +  Z  ft  -  [Z  h  +  gs|  R’1  [hZ  t  +  stgt]  -  [z ht  +  gs]{[Z  h  +  gs]  r"1)  T 
+  [Zh  +  gsJr’1  r{[Zh  +  GsIr-1)1  +  gqgt, 


or 


(C.51) 


C  .4  SUMMARY 


The  results  of  the  Kalman-Bucy  paper  have  been  extended  to  include 


-13L 


the  case  wherein  there  is  correlation  between  the  input  process  noise 
and  the  measurement  noise.  The  equations  of  interest  are  shown’  in 
Table  C.l. 

/ 

Table  C.l 

SUMMARY  OF  KALMAN- BUCY  ESTIMATOR  WITH  CORRELATED 
INPUT-MEASUREMENT  NOISE 


Message  model 

x(t)  =  F(t)  x(t)  +  G(t)  u(t) 

Observation  model 

z(t)  =  H(t)  x(t)  h  v(t) 

A  priori  statistics 

E  |u(t)j  =  0 

E  |u(t) ,  uT(t)|  t  Qfs(t  -  T> 
Ejv(t)|  -  0 

E Jv(t) ,  vT(t)|  =  Rfs (t  -  T) 
E|u(t),  vT(t)|  =  S5(t  -  t) 

Process  noise 

Measurement  noise 

Correlated  process 
and  measurement  noise 

Filter  algorithm 

x(t)  =  F(t)  x(t)  +  K(t)[z(t)  -  H(t)  x(t)] 

Optimal  gain 
algorithm 

K(t)  =  [7 (t)  HT(t)  +  G (t)  S(t)]  R_1  (t) 

Error  variance 
algorithm 

f(t)  =  F(t)  T <t)  +  T (t)  FT(t)  -  [7 (t)  HT(t) 

+  G(t)  S (t )]  R_1(t)[7(t)  HT(t)  +  G(t)  S(t)]T 

+  G(t)  Q(t)  G(t) 

Initial  conditions 

x(0)  =  E|x(to)j  =,lx(o) 

7(0)  =  E|jx(t0)  -  x(to)J  [x(to)  -  x(tQ)  TJJ=-  Ox(o) 
x(0)  -  x(tQ) 

Appendix  D 

COMPUTER  PROGRAM  LISTING  AND  SAMPLE  OUTPUT 

The  listing  is  given  for  the  five  measurement.  Case  II,  with  fixed 
gains.  The  program  is  written  in  Fortran  IV  using  double  precision 
arithmetic. 

The  MAIN  program  defines  the  initial  conditions  on  the  states  and 
the  constant  values.  It  then  establishes  the  covariance  matrix  Riccafi 
equation  as  well  as  temporary  storage  locations  for  the  flow  of  data 
when  the  program  commences.  The  measurement  matrix  (H),  is  established 
as  a  set  of  three  row  vectors  to  give  a  sequential  updating.  Though 
not  a  savings  in  this  case,  it  is  established  for  future  studies 
wherein  correlated  input-measurement  noise  is  to  be  investigated.  After 
the  measurement  update  sequence  is  completed,  the  updated  states  and 
covariances  are  used  as  new  initial  conditions  for  the  differential 
equations  propagated  in  subroutine  DIFFEQ .  The  output  is  formatted  for 
the  printout  and  finally  the  plot  routine  is  prepared  to  accept  data  for 
storage  on  tape  for  later  graphing  off  line. 

Subroutine  DIFFEQ  is  the  subroutine  which  specifies  the  form  of 
the  differential  equations .  It  computes  elements  of  the  F  matrix  about 
the  current  estimate  rather  than  about  a  nominal  trajectory  because 
the  latter  is  undefined.  The  random  noise  injected  into  the  system 
equations  is  also  defined  in  this  subroutine.  It  uses  random  numbers 
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generated  with  a  uniform  distribution  in  subroutine  RANDU  to  obtain 
random  numbers  with  a  Gaussian  distribution  using  the  Box-Miiller 
transformation. 

Subroutine  RUK  is  a  fourth-order  Runga-Kutta  quaterature  routine 
which  integrates  the  differential  equations. 

Subroutine  SORT  is  a  matrix  conditioner  to  prepare  the  measurement 
matrix  for  call  into  the  update  sequencing.  Again,  though  it  is  not 
necessary  in  this  case,  it  is  available  for  future  studies  when  the 
rows  of  H  are  no  longer  merely  a  single  constant,  unity,  and  a  list 
of  zeros . 

Subroutine  RANDU  is  a  random  number  generator.  It  generates 
random  numbers  with  a  uniform  distribution  in  the  closed  set  [0,  1] . 

The  subroutine  BLOCK  DATA  sets  up  initial  conditions  throughout 
the  program  prior  to  any  computations  in  the  algorithms.  It  is  an 
efficient  way  to  initialize  constants  which  are  common  to  many 
subroutines. 

The  computer  printout  shown  in  the  last  two  pages  of  text  are 
representative  of  the  data  obtained.  The  last  page  is  the  printout 
at  time  t  =  10  seconds  into  the  simulated  flight.  The  page  previous 
to  it  indicates  the  initial  conditions  at  time  t  =  0  second. 


**  5  ..J;STIM*r,9v  9F  INERT:At,  system  errors 

3t  C  USING  TERMINAL  GUIDANCE  POSITION  MEASUREMENTS 
**  £  VIA  A  RADAR  MAP  MATCHING  SYSTEM 

61  C  THIS  PROGRAM  3*  CRATES  A  C0N9TA.NT  VFLBCITV  nrofrMT  to*  tppvaav  > 

71  C  COMMENCING  AT  30# 300  FT*  FOR  A  RE«ENTRY  BODY*  AM  EXTENSION  OF  THE 
JJ  £  KALMAN»BUCY  FILTER  ESTIMATES  THE  ERROR  IN  AM  INERTIAL  SYSTEM,  THE 

10  C  lcNTTES?iMAT?LLr5f*IS  $KN  ^  ****«»  ABOUT  THE  WU 

Si  e  ^  ^ ^  fHE^MEASUREMENTS^S I MULCTED*  AS^A  * RADAR^MAP  >MATCH!  NG  »U, 
131  C  AND  T9  SIMULATE  THE  N9JSE  IM  THE  INPUT  PROCESS.  A  PLOT  RBUTtSr 

1AI  C*#*###I####I*TH£  EN!>  ®P  MA1M  #R9G,AM  35MERATES  THE  9JTPJT  GRAPHS, 

ISI  C  DEFINITIONS  OF  THE  0IMENSI9ME0  VARIABLES 

i7t  £  *<*»  STATES  OF  the  SYSTEM 

r  DIFFERENTIALS  9F  THE  STATES 

19x  C  SAAV( !# J)  A  STATE  A^RAY  SAVO  £ACM  IVC^P'J-NT  Ttmr 

S!  S  m!-"  TE"P??A,V  5T??E  r*  ^ 

II!  I  ”  ;:  £?;?;  :;  ::  tk  «*»?»'«  tww. 

Is!  I  Kiil'.j!  J.”?  vee!”  ?e  vutm<vn  seswwms 

8bl  C  HR0W3( I# J)  »»  m  «t  ii  ii  ,, 

z7t  c  mi i )  the  measurement  VALUES 

?8!  C  ST9<I)  ST9RA3E  OF  M£ASURe‘Ms'NTS  AND  SAINS 

S  STSRA3E  9P  OPTIMAL  3AINS  FOR  THE  ENTIRE  RUM 

Ill  r  vmc*S?t!  STORAGE  9*  COVARIANCE  AFTER  A  MEASUREMENT  UPDATE 

311  C  XMSAvm  M  »*  STATES  '•  I  i  »»  m 

II! '  E",x''’  PSSSSMrSSn**™1  E,T',*TE!>  S,,TES  ,rrE* 

3AI  C  ERRXMU)  DIFFERENCE  BETWEEN  ACTUAL  AND  ESTIMATED  STATES 

C  BEF9RE  A  M£AS JREMENT  UPDATE 

r  DYNAMIC  SYSTEM  F  MATRIX 

371  C  FP{ I, J)  MULTIPLIED  F  AND  p  MATRICES 

3SI  C  DQDT( I«U)  MULTIPLIED  D.Q.AND  D  TRANSPOSE  MATRICES 


X(I) 

DX<  1 1 
SAAV| t*J) 
SAAVUP(I.U) 
xsaav<i) 

PSAAV1I) 
PII.UI 
HROWj (ItJ) 
HR9W2(I,J) 
HR9W3( I# J) 
H(I) 

ST9<I) 

RK9P(!#U> 

PPLU89I) 

XMSAV(l) 

ERRX(I) 

ERRXMH) 

F(I# J) 

FP{ I, J> 

OODT ( T.U) 


*01  C  DEFINITIONS  of  THE  N9NDIMENSI0NED  VARIABLES 


*11  c 
*ai  c 
*31  c 
**:  c 

*55  C 
*61  C 
*71  C 

*81  c 
*91  c 
501  C 

SI  I  c 

521  C 
531  C 

s*:  c 

551  C 
561  C 
571  C 
581  C 
591  C 
601  C 
611  C 
621  C 


XI 

Y2 

Z3 

XP6 

YP7 

ZP8 

RX 

RY 

RZ 

SX 

QY 

3Z 

omega 

DEG 

GE 

RE 

ELAT 

PI 


X-POSITISN  ERR9P  OF  INS  AS  A  STAT* 

Y-P0SITI9N  ii  »i  ii  it  ••  ~ 

Z-POSITION  *•  • l  it  M  ii 

XI  STATE  PERTURBED  wjth  nOIS* 

Y2  ii  ii  ii  ,i~ 

Z3  • •  ••  »i  M 

SPECTRAL  DENSITY  OF  MEASUREMENT  NOTS?  IN  X 
M  *  *  *  *  i*  m”  1 1  y 

* •  ii  • i  1 1  •  •  •  i  2 

"  "  '»  PROCESS  •»  ii  X 

•  •  *  *  *  i  M  1 1  1 1  y 

*  i  *  *  *  I  II  M  M  7 

DE3RE£S  *',aJLA,?  VELOCITY  IN  AN  INERTIAL  BASIS 

EARTH’S  ACCELERATION  OP  3RAVITY 

EARTH'S  RADIUS 

LATITUDE 

CONSTANT  IN  RADIANS 


IUPDAT  the  NUMBER  OF  MPASUREMENT  93SFRVATI9NS 
IX  RANDOM  NUMBER  for  the  MONTE  CARLO  SIMULATION 
DT  INTEGRATION  STEP  SIZE 


Mi 

**!  c  Mate:  aCl  other  variables  are  used  ra  shorten  the 

451  C  ALSEBRAfC  EXPRESSIONS  AND  HENCE1  ARE  SELF  EXPLANATORY. 

461  £#»*•••»♦###*• 

471  C  HA IN  PR03RAM  FOLLOWS 
681 

491  IMPLICIT  REAL*#  IA-HjB-Z) 

70|  REAL*#  X(26)#DX(26) 

71 1  C8MH0N/ARRA VI /SAAV ( 26# 58) #  SAA V JPC 26 ) # X9A AV (5 ) # 

721  1  PSAAVI15)#PI5#5) 

731  CBMM8N/ARRA Y2/HR8W1 ( 1#  51  * HR8W2I 1 « 5  >  # MR8W3C 1 *  5 ) #  HIS ) # ST8I 5)# 

741  1  RK8PT 1 5#  5S ) #  PPLUS ( 25 ) #  XMS A V ( 5  f 

75 »  C6MM»N/ARRA  V3/ER&X  <  5 ) j  ERRXM 1 5  > 

76  X  COMMON/ ARRA Y4/F 1 5#  5 ) #  FP 1 5#  5  >  <  03DT 1 5#  5 ) 

771  COMMON/Bl-/  XI  #  Yf  #  Z3»  XP6#  Y®7#  ZPg#  RX#  RY#  RZ 

781  C8MMaH/Cl/9HEQA#eLAT#oeS*aE#RE#P!#T#OELTAT«ICaUHT#IHAX«IUPoAT#NS 

791  C8MM8N/C2/IX#LIM 

SOX  EXTERNAL  RANDU 

#11  C 

821  C  CONSTANTS  INITIALIZED  IN  THIS  BLOCK. 

83  X  C 

841  0ME3A*15. 0410700 

851  OEO*0. 0174532900 

86 X  82*32*172400 

871  RE*63783BS.90*3.28100 

881  ELATb45.D0 

891  PI •3*141592653600 

901  IUPDAT«10 

911  LIM-51 

92!  NS«26 

931  IX«4000 

941  DT»0.290 

95!  C 

96!  C  THESE  ARE  A90ITIBNAL  NBN  ZER9  IC  . 

97!  C  N9MINAL  STATE  9N  FIRST  PASS#EST!NATE9  STATE  THEREAFTER. 

98!  C 

99!  08  1911  J*1#NS 

190!  1011  XiJ)«0*00 

101!  04  1912  J*1«NS 

192!  1012  DXI J)*0.99 
193!  C 

194!  C  ACTUAL  STATE  IC. 

195!  C 

196!  X(6)at253.90 

197!  XI7)«1317.99 

108!  X(S)*1590O9 

1991  X(9)«i0*09 

110!  Xtl0>*10.90 

111!  C 

112!  C  STATE  ERR9R  C9VARIANCE  IC#  JPPER  TRIA.N3LE' ONLY* 

113!  C 

1141  X( 1 1 ) .lOOOO.OO 

1151  X(13)al0030.00 

116!  X(16)al9090.00 

1171  X(20>»0. 0009190 

118!  X<25>«0* 0000190 

119!  C 

1201  C  INTEGRATION  9F  MAIN  E0UATI4NS 
121!  C 

1221  C  RMAX  A*n  IMAX  EQUAL  T9TAL  4*  FL13HT.T/9IVI9E0  BY  INTS3RATI9N 
123!  C  STEP  SIZE  OELTA.T.  <10  AN9  110  ARE  VARIED  ACC4R9IN3  T9  THE 
124!  C  NUMBER  9F  INTE3RATI4N  STEPS  AN9  OELTA-T  SIZE. 
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125)  C 

126)  RMAX*50»D0 

127)  IMAX»IDINT<RMAX) 

128)  C  X10«!MAX/)UPOAT 

129)  K10*ll 

1301  IC8URT«1 

131)  09  125  M5*1#NS 

132)  09  125  N5»1.56 

133)  125  SAAV(M5#R5)»O.DO 

1341  09  1009  110*1)6 

135)  119  OK  120  JW.'^S 

1361  NS1 

137)  ISO  SAAVJ  Ji f 1  COUNT )*X(J1 ) 

1381  IC9UNT*IC9jRT*t 

1391  C 

1401  122  CALL  RUKIX.OT.RSI) 

1411  C 

1421  IF  (ICOURT-KIO)  119) 200# 200 

1431  200  CONTINUE 

1441  09  124  M4*1«RS 

1451  124  SAAV<M4.IC9UNT)*X<M4) 

146)  C 

1471  C  SET  READY  FOR  THE  MEASUREMENT  UPDATE.  THE  H.MATRIX  IS 
1481  C  TREATED  AS  3  SEPARATE  R9WS  OF  H  IN  ORDER  T9  SAVE  OR  MATRIX 
1491  C  MULTIPLICATION.  EACH  INTEGRATOR  IRTERVAL  IS  STORED  IR  ARRAY 
1501  C  NAMED  SAAV. 

1511  09  150  K*1«RS 

1521  159  SAAVJP(K)*0t00 

1531  09  151  I *1#RS 

151.41  151  SAAVJR(I)*SAAV(I»IC9UNT) 

1551  C 

1561  C  XHAT  MINUS  IS  TAKEN  FR9M  SAAVUP  ARO  ®LACEO  IR  TEM®9RARY  STORAGE* 

1571  C 

1581  D9  152  J*l«5 

1591  152  XSAAV ( J) *0.00 

1601  09  153  K*l«5 

1611  153  XSAAV(K)*SAAVUP<K) 

1621  C 

1631  C  P  MINUS  IS  TAKER  PR»M  SAAVJ®  ARO  PLACED  IR  AETHER  TEMPORARY  STORAGE. 
1641  C 

1651  D»  154  J*1«15 

1661  154  PSAAV(J)*0.90 

1671  M1*0 

168!  00  155  K.11,25 

1691  MIpMI^I 

170!  155  PSAAV(M1)»SAAV'JP{K) 

1711  C 

172!  C  PLACE  P  MINUS  IRT9  5X5  MATRIX  P9R  MEASUREMENT  UpOATE. 

1731  C 

1741  08  155  1-1*5 

175!  09  156  J»l)5 

1761  156  P(I»J)*O.DO 

1771  <1*0 

1781  09  157  M*l*5 

1791  09  157  R*l) M 

1801  K1*K1*1 

1811  P<RjM)*PSAAV<K1> 

1821  157  P(M«M)*P(R)M) 

183!  C 

184!  C  DEFINE  H  AS  A  SET  Op  R9W  VECTORS  INSTEAD  OF  A  SINGLE  MATRIX. 

1851  C  THESE  ARE  ITS  IC. 

186!  C 
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1*71 

1  881 

1891 

190! 

C 

191! 

C 

192! 

c 

193! 

1941 

195! 

196! 

C 

197! 

c 

198! 

c 

199! 

POO! 

201! 

202! 

203! 

P04 ! 

P05! 

c 

P06 ! 

r 

A* 

207! 

c 

208! 

209! 

105 

?10! 

211! 

101 

212! 

213! 

214! 

215! 

216! 

217! 

c 

218! 

C 

219! 

C 

220! 

C 

221! 

c 

222! 

223! 

224! 

225! 

226! 

227! 

300 

228! 

229! 

310 

230! 

231! 

232! 

30 

233! 

c 

23m  : 

C 

235! 

C 

236! 

237! 

102 

238! 

239! 

240! 

241! 

242! 

243! 

C 

244! 

245! 

246! 

247! 

248! 

HR8W1 d*  1 )  *1 . DO 
HR8W2(l*2).i  .00 
HR8W3d*3)*l.D0 

MEASUREMENT  ERR8R  MATRIX  IN  C8LJMN  ?8RM. 

ERR8R1«XP6*RX-1.00*X1  V 

ERROR?" YP7*RY-1.D0*Y2 

ERR8R3.ZP8*RZ-l.DO#Z3 

EXPECTED  VALJE  8F  MEASUREMENT  N8ISE. 

SIGRX.50.D0 

PR901 "SIGRX.SIGRX 

SIGRY.50.D0 

PR9D?«SIQRY»stGRY 

SIGRZ.100.D0 

PR903.SIGRZ.SI3RZ 

THIS  IS  FIRST  INCREMENT  9F  THE  MEASUREMENT  UPDATE  SEQUENCE. 

08  105  LL1.1.5 

H(LL1 >*o.oo 
09  101  L 1 • 1 . 5 
H(L1 ).HR0W1 d*Ll ) 

CALL  SORT (a.H* STS* 5* 5* 1.5* 2* 25. 5. 5) 

CALL  S9RT(H,ST9#hPHT*1*5.5*1*1»S»3*1  > 

IF  <HPHT.E3,0)  HPHT. 1.0-15 
HPhT*HPHT*PR5D1 
08  30  1*1.5 

FOR  THE  FIXED  3AIMS  CASE.  H(I)  IS  9Y»ASSED  WITH 
COMMENT  AMO  THr  CONSTANTS  ARr  INSERTED.  THr  SAM 
IS  TRUE  F9R  THr  ?N0  AND  3RD  INCREMENTS  9*  J»DATE 
the  case  here  IS  E9R  The  5  JPDATE  SE3JENCE. 

H{ I )*ST9( I J/HPHT 
H(1 )«0«16D»03 
H(2)«-0.40D-03 
H(3)«D.93D0 
H(4 ) **0. 050*01 

IFdC0UNT.3E.Al)  H(A)«.D.4D»D1 
H(5).0O8D0 

IF(ICAJNT.3e.31)  H<5)»*3»16D0 
CONTI NJE 

XSAAV(I)*X3AAV(I)*H(I)*rRR0Rl 
08  30  J*1.5 

P(t.J).P(I. J).H(I)*3T0< J> 

THIS  IS  SECOND  INCREMENT  Or  Thc  MEASUREMENT  UPDATE  SEDJ-NCE. 

D8  10?  L2«l»5 
H(L2).HR0W2( 1.L2  > 

CALL  SORT (P.H.ST9. 3.5. 1.5*2* 25.5. 5) 

CALL  SORT ( H.ST9.HPHT* 1 *5* 5* 1*1*5* 5*1) 

IF  (HPHT.E3.0)  HPHT«1.D-15 

HPhT»HPHT*P°80P 

D8  31  I » 1  *  5 

H(  I  ) .STS ( I J/HPHT 

H(1 ).0»16D>03 

H(?)«-0.a0D-03 

H( 3 )*0«93DO 

HI4).»0.0SD»01 

IFdCOUNT.3r.41 )  H<4)«. 0.001 
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I/>  I'l 


249!  3?0 
250! 

251 !  330 

2521 

253: 


H(5>*0.0800 

IF{ IC9JNT.3E.31 )  H(5)*O.16D0 

CSNTtNUc 

XSAAV { I ) *XSAAV( I )4H( I ) *?RR8R2 
08  31  J*l»5 


254 

255 

256 

257 

258 

259 

260 
261 
262 

263 

264 

265 

266 

267 

268 

269 

270 

271 

272 
273: 
2741 
275! 
276! 
277! 
2781 
2791 
280: 
281 1 
2821 
283! 
284! 
285! 
286! 
2871 
2881 
2891 
2901 
2911 
292! 
293! 
2941 
2951 
2961 
2971 
298! 
299! 
300! 
301! 
302! 
3031 
3041 
305! 
306! 
307! 
3081 
3091 
3101 


31  P(I#J)*P<I.J>-H<!>*ST9<J) 

C 

C  THIS  IS  THIRD  INCREMENT  9F  THE  MEASUREMENT  'JPOATE  SE3J-NC5* 

C 

08  103  L3*1.5 

103  H(L3>*HR9W3<1#L3) 

CALL  S9RT<p,H.ST9>5»5.1,5*2#25.5»5) 

CALL  S9RT ( H.ST9. HPHT /  li  5>  5/  1#  1 ,  5.  3.  1 ) 

IF  (HPHT.E3.C)  HPHT-1.0-15 

HPHT»HPHT*pr»D3 

08  32  I  * 1 #  S 

C  H<n*ST9(I  )/HPHT 
4(1  )«0«160*03 
H<2)». 0.400-03 
H(3>»D.93D0 
H(4)«-0. 050-01 

IFUC9UNT.3E.41)  H(4)»-3.4D-0l 
340  H(5)*0»08D0 

IF ( IC4JNT.3E.3l )  4(5)  ■♦0»1600 

350  CBNTINUE 

RK9PT ( I i 1E9UNT )*H( I ) 

XSAAV ( I ) *XSAAV< I )+H< I )**RR9R3 
08  32  J*l#5 

32  P(I.U)*P<I.U)-H(!)*ST9(J> 

06  106  <2*1.23 

106  PPLUS(K2)*0.00 

K3*10 

08  104  13*1,5 
08  104  U3-1.I3 
<3»K3*1 

104  PPLUS(<3)«B(J3#I3) 

C 

C  AT  THIS  °9lNT  T4E  MEASUREMENT  'JPOATE  SEQUENCF  IS  C9M®LrTr0» 

C  THE  VALUE  F9R  V  HAT  PLJS  IS  ST9RED  IV  THE  LAST  XSAAV(I),  THE  VALUES 
C  P  PLUS  ARE  ST9RE0  IN  THE  LAST  PPL'USU),  ANO  THE  OPTIMAL  SAINS  APE 
C  IN  KBPT(I). 

C 

C  SET  THE  UPDATED  STATES  AND  C8VARIANCES  BACK  INT9  A  F9RHAT 
C  SUITABLE  AS  THE  NEW  IC*S  F9R  TIME  PR9P98ATI9N  VIM  INTE3RATI9N  9F  THE 
C  DIFFERENTIAL  E3UATI8NS. 

C 

C  THIS  IS  F9R  XHAT  PLUS. 

C 

00  162  <11*1.5 

162  X(K11)*XSAAV(K11) 

C 

C  THIS  CBlCECTS  X  PERTURBED 
C 

08  164  Mll*6,10 
164  X(H11)*SAAVUP(M11) 

C 

C  THIS  IS  C9VARIANCE  PLUS. 

C 

08  163  LIi-11,25 

163  xuimpPLUsain 

c 

C  THIS  IS  TIME 


lilt 

3131 

313: 

3141 

3151 

316! 

317! 

318! 

3191 

330! 

331! 

332! 

333! 

334! 

335! 

336! 

337! 

338! 

339! 

3301 

331! 

332! 

333! 

334! 

335! 

336! 

3371 

338! 

339! 

340! 

341! 

342! 

343! 

3*4! 

3451 

346! 

347! 

348! 

349! 

350! 

351! 

352! 

3531 

354! 

355! 

356! 

337! 

358! 

359! 

360! 

361! 

362! 

363! 

364! 

365! 

366! 

367! 

368! 

369! 

370! 

371! 

372! 


165  X<26)»SAAVJP<26) 

C 

C  THIS  BLACK  FORMATS  THE  0UT»JT. 

C 

M»1 

IF  (K13.NiS.ll)  M.K10-13 

701  FORMAT ( * 1 • # /#5X#  *T«'#D1?«4#2X#  » SAAV  I NDEX» « # 14# ✓) 

WRITF(6#731)  SAAV(26#M)#M 

C 

702  FORMAT (»  *#3X#*THE  ACTUAL  ERROR  IN  THE  INERTIAL  NAVIGATOR  #!£#*# 

1  ' THF  PERTJRBE3  STATE  C) 

WRf YE(6#  702) 

703  FORMAT ( '  *#SX#*XP  «'#D17.10»2X# • V*  «*#D17.10#2X# »ZP  •* *917.10* 2X« 
1  'VXP»'*917.10»2Xi«VZP»**9i7.10»///> 

WRITF<6*733)  SAAV(6#‘1)#SAAV{7#M)»SAAV(8#9)#SAAV<9#‘')#SAAV(10»'O 

r 

704  FORMAT ( *  «*5X#*ThE  ESTIMATED  ImERTIAL  NAVIGATOR  ERROR  (STATE)* 

1  •  BCF5RC  A  MEASUREMENT  JPDATE  C) 

WRITE(S#  704) 

C 

705  FORMATC  *#5X#»XE  • **D17.10*2X# • VE  •*#317.10»2X#  *ZE  •»  *917.10»2X* 
1  'VXE»**017.10*2x»*VZE«*#917.13»///> 

WRI TF  (6#  705 )  SAAV<1*M>»SAAV(2*M>#SAAV(3.M)*8AAV(4*M>#SAAV(5*M> 

C 

5051  FORMATC  *#5X#«THE  ESTIMATED  INERTIAL  NAVIGATOR  ERROR  (STATE)*# 

1  *  AFTER  A  MEASUREMENT  JB3ATE  !') 

WRITE(6#505l) 

C 

5052  FORMATC  *,5X#iXE  •*#017.10#2X, « VE  •  •  #017. 10# 2X» »ZE  ■  •  »017.10#2X# 
1  *VXE»*#D17.10#2X#  *VZE»»#017.10#///) 

WRITE (6, 5052)  XSAAVd >*XSAAVC2),XSAAV{3>,XSAAV(4>.XSAAV{5) 

C 

506  FORMAT ( •  »,5X#*THE  FILTER  ERROR  IN  ESTIMATED  STATE  BEFORE*# 

1  *  TM?  MEASUREMENT  JPDATE  I  *  > 

WRITE (6#506) 

C 

00  601  L-1.5 
N-L-45 

601  ERRX(t)*SAAV(L»M)-SAAV(N«M> 

507  FORMATC  • #5X# *ERRX» ' #917. 10# 2X# ' ERRY» * #D17» 10*2X'ERRZ»* #917.10# 

1  2X,*ERRVX*«,D17.10#2X#*ERRVZ«'#017.10#///) 

WRITF (6#507>  ERRX(1 )#ERRX(2)#ERRX(3)#ERRX(4)»ERRX(5) 

C 

508  FORMAT ( *  *«5X#*THE  FILTER  ERROR  IN  ESTIMATES  STATE*# 

1  *  AFTER  the  MEASUREMENT  UPDATE  !*) 

WRITE<6#508) 

C 

KK-0 

00  602  L«6« 10 
KK*KK4l 

602  XMSAV(KK)»SAAVUP(L) 

DO  603  N«l#5 

603  £RRXM(N)«XSAAV(N)-XMSAV(N) 

c 

509  FORMATC  » #5X# »ERRX**#D17* 10# 2X# *SRRY*« #D17#10#2X# 'ERRZ** #917.10# 
1  2X»*CRRVX*i,017.10#2X#«SRRVZ»«#D17.10#///> 

WRITE(6#509)  ERRXMd)#ERRXM(2)#ERRXM(3)#ERRXM(4)#ERRXM(5) 

C 

713  FORMATC  *#5X#*TME  ERROR  COVARIANCE  MATRIX  BEFORE* # 

1  »  A  MEASUREMENT  UPDATE  «») 
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3731  WRITF<6#710) 

3741  C 

3751  711  FORMAT!  •  *#5X#*Pll  •« #017.10# 2X# • »12  •*#017.10#2X# *»13  •**017*10# 
376!  1  2X#*P14  •*#017.10*2X#*B15  •**017*10) 

377)  WRIT?  (6*711)  SAAV<ll#M>,SAAV<l2,M)#SAAV<14#M)*SAAV:i7#M>» 

378:  1  SAAV!21#M) 

379:  C 

3R0:  712  FORMAT !  *  *#29X*'P22  •* *017.10*2X» * P23  •*»017*10#2X# *P?4  •*#017.10* 

38i:  1  2X#  » *25  •*,017*10) 

3821  WRITF  (6*712)  9AAV<13#M>,SAAV{1S.W)#SAAV<18#M)#SAAV(22#M) 

383:  C 

3841  713  FORMAT ( '  *#53X*'P33  •* #017.10#2X» *P34  • • #017. 10# 2X# • P35  •*#017.10) 

385:  WR!T£!6*713)  «?AAV:i6#M>,SAAV!l9.M>»SAAV(23#M) 

3861  C 

3871  714  FORMAT ( '  **77X#*P44  •*#017.10#2X# *P45  •♦#017*10) 

3881  WR1TF(6#714)  SAAV<20*M)#SAAV{24.W) 

3891  C 

3901  715  FORMAT!*  *#101X#*P55  ■*#017*10#///) 

3911  WR!T£!6#715>  BAAV(25#M> 

3921  C 

3931  516  FORMAT!*  *#5X#*THE  ERROR  COVARIAVCE  MATRIX  AFTER*# 

3941  1  *  A  MEASJREME'JT  UPDATE  1*) 

3951  WRITfl6#516) 

3961  C 

3971  517  FORMAT:*  *#5X,*P11  •*#017»10#2X# *®12  •*#017*10#2X# *P13  ■*#017*10# 
3981  1  2X#*P14  ■' #017*10# 2X*  *P15  •*#017.10) 

3991  WR I TE !  6#  51 7 )  PPLUS ( 1 1 ) #  PPLUS  < 1 2  >  # P*LUS 114)#  PPLUS 1 1 7  >  #  PPLUS 1 21 ) 

4001  C 

*011  518  FORMAT:*  *  #29X#  *P22  »*#017.10#2X, *P23  •*.017*10#2X# *P24  •*#017.10# 
4021  1  2X**P25  ■*#017*10) 

4031  WR1TF!6#518)  PPLUS113)#RPLUSU5)#PBUUS<1S)#PPLUS122) 

*041  C 

4051  519  FORMAT!'  *#53X#  *P33  »*#017.10*2X»*P34  •*#017.10#2X# *P35  ••#017.10) 
4061  WR!TF(6#519)  PPLUS<16>.PPLUS!19)#PPLUSI23) 

4071  C 

4081  520  FORMAT!'  *#77X#*P44  ■*#017.10*2X# *P45  •*#017.10) 

4091  WR1TE16#520)  PPLUS<20)»®PL'JS124) 

4101  C 

4111  521  FORMAT!'  • # 101X# *P55  •*#017.10#///) 

4121  WRIT? (6#521 )  PPLUS l 25) 

4131  C 

4141  522  FORMAT! '  *#5X#*THE  OPTICAL  TIWE»VARY!N3  FILTER  3AINS  1*) 

4151  WR|TF10#522) 

4161  C 

4171  523  F0RMAT1*  *#5X#*XX  •*#D17.10#2X# *XY  •«#017*10#2X# *<2  •*#017.10# 

4181  1  2X#»<VX«i#017.10#2X#*KVZ«»#017.10*/) 

4191  MRITC(0#523i  RK0PT:i#M),RX0PTl2,M>#R<0PT(3#M)#RK9PT14#M), 

4201  1  RK0PT«5#M> 

4211  K10«K10+10 

4221  1000  CONTINUE 

4231  500  CONTINUE 

4241  C 

4251  C  THE  NEXT  BLOC*  SETS  UP  THE  PLOT  ROUTINE  FOR  3RAPHIN3. 

4761  C 
4271  C 

4281  C  DATA  FOR  THE  FIRST  3RAPH  <X«P0SITT5N)  FOLLOWS. 

4291  C 

4301  00  41  I«l«5l 

4311  X< I ) vSAAVI 26# I ) 

4321  Y*(8AAV( 1# I ) *SAAV( 6# I ) ) 

4331  CALL  P0INT111#X#Y) 

4341  Y*0SQRT(8AAV1 11# 1)1 
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4351 
4361 
4371 
438i 
4391 
4401 
4411 
442  1 
4431 
4441 
4451 
446: 
4471 
4481 
4491 
4501 
4511 
4521 
4531 
4541 
4551 
4561 
4571 
4581 
4591 
4601 
4611 
4621 
4631 
4641 
4651 
4661 
4671 
4681 
4691 
4701 
4711 
4721 
4731 
4741 
4751 
4761 
4771 
4781 
4791 
4R01 
4811 
4821 
4831 
4841 
4851 
4861 
4871 
4881 
4891 
4901 
4911 
4921 
4931 
4941 
4951 
496! 


CALLPftIMTl (2#X» Y) 

Y«.Y 

CALL  “ftl^Tl (3#X#Y) 

4i  cbstivue 

CALL  CJRVE1U#1>0> 

CALL  CJRVEi ( 2# 1# 0) 

CALL  CJRVEI ( 3# 1  * 0) 

CALL  P9IMT1(4#0«*2.E4) 

CALL  P5W1(4,11.#.2.E4) 

CALL  CURVE1<4#0»3> 

CALL  TITLE? (43# 'FILTER  ERRftR  5  VARIAVCEt  X(FT>  VS  TIME(SEC)*) 
CALL  33APm(U.#tTI'1E'#10.#»  X  •> 

C 

C  OATA  Fft9  THE  SECftMD  GRAPH  ( Y-P9SITI9M)  P9LL5WS* 

C 

Oft  42  I  • 1 #  5 1 
X<n.SAAV(26#I> 

Y«(SAAV(2#I)-SAAV(7#f n 
CALL  P9IHT1 ( 1#X# Y) 

Y«0S39T(SAAV(13#in 

CALLP»W1(2#X#Y) 

Y«.Y 

CALL  P9IMT1 ( 3# X# Y) 

4?  C8MTTMUE 

CALL  CJRVEI ( 1# 1 #0) 

CALL  CJRVEI ( 2# 1 # 0) 

CALL  CURVE1(3#1#0> 

CALL  P9IHT1(4,0.#2.E4> 

CALL  P9INTl(4#ll.i«2.E4) 

CALL  eURVEl<4#0*3> 

CALL  TITLE2(43# ' FILTER  * RR8R  5  VARIAMCEl  Y<m  VS  TIHE(SEC)') 
CALL  SRAPH1(11.#«T!hE»#10«#*  Y  *) 

c 

c  OATA  PB9  THE  TH1R0  GRAPH  (7-P9SlTt9M)  F9LL9WS. 

C 

Gft  Tft  430 
431  OB  43  1-1*51 

X( I )«8AAV( 26# I ) 

Y»  (SAAV(3# I ) «SAAV(S# I )  ) 

CALL  P9IHT1 < 1#X# Y) 

YOS3RT  (SAAV(  16#  I ) ) 

CALLPftI':ri(?,X.Y) 

Y»-Y 

CALL  ®9IHT1(3#X#Y> 

43  CBNTIHUE 

CALL  CURVEl(l#l#0) 

CALL  C'JRVE1(2#1#0) 

CALL  CURVE1<3#1#0> 

CALL  P9IMTl(4#0.#10tE3) 

CALL  P9INT1(4#11##»10.E3) 

CALL  C'JRVE1(4#0#3) 

CALL  TITLE2(43# 'FILTER  ERR8R  5  VARIAMCEl  Z(FT)  VS  TIHE(SEC)*) 
CALL  SRAPHmi«#'T!HE»#10.#»  X  ') 

430  CBMTIMJE 
C 

C  OATA  F9R  THE  FftURTH  GRAPH  (X-VEL9CITY)  F9LLBWS* 

C 

09  44  m#51 
X( I )»SAAV( 26# I ) 

Y» ( SAAV(4» I ) «SAAV( 9# I ) ) 

CALL  P9INTI ( 1#X# Y) 

Y*OSGRT ( SA A V ( 20# I >  > 
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*§71  CALLPBINT1 (2#X« Y) 

*981  Y*»Y 

*991  CALL  P9INT1(3*X*Y) 

5001  **  CONTINUE  f 
501 t  CALL  CURVE1<1j1#0) 

5021  CALL  CURVE1<2>1#0) 

5031  CALL  CURVEl(3«t*0) 

50*1  CALL  P81NT1I*»0»#3*£3) 

505!  CALL  P9lNTi(*#llo*3.E3) 

5061  CALL  CURVE1I*«0»3> 

5071  CALL  TITLE2<*5. ‘FILTER  ERROR  5  VARIANCE!  VX(FPS)  VS  TtME(SSCP) 

508!  CALL  3RAPH1(11.»  «TIME»*10»*  •  VX  ») 

509!  C 

5101  C  OATA  FOR  THE  FIFTH  GRAPH  tZ*VEL8CITY>  FOLLOWS* 

5111  C 

512!  08  *5  I » 1 # 51 

513!  X( 1 ) «SAAV( 26* !  ) 

51*1  Y» (SAAV<5# 1 )«SAAV( 10* I ) ) 

515!  CALL  P8INT1(1»X*YJ 

516!  Y»3S3RT(SAAV(25*I)) 

5171  CALLP0INT1<2*X,Y) 

518!  Y««Y 

519!  CALL  P0INTl<3,X»Y) 

520!  *5  CONTINUE 

521!  CALL  £URVE1(1«1*0) 

522!  CALL  CURV£1(2*1*0) 

523!  CALL  CURVE1 13*1*0) 

52*1  CALL  P9!NTl(*f3*«2*E3) 

5251  CALL  PBINT1 ( *» 11 «*«2«E3) 

526!  CALL  CJRVE1<*»0»3> 

527!  CALL  TITLE2! *5#  *  FILTER  ERROR  5  VARIANCE*  VZ(FPS)  VS  TIME(SEC)') 

528!  CAUL  3RAPH1<11.»»TIME»*10«#'  VZ  ») 

5291  C 

530!  C  OATA  FOR  THE  SIXTH  GRAPH  «X  OPTIMA^)  f5LL9WS» 

531!  C 

532!  08  *6  1*1*51 

533!  X(I)>SAAV(26#T) 

53*1  Y«RK»PT<1#I) 

535!  CALL  P0INT1 { 1*X» Y) 

536!  46  CONTINUE 

537!  CALL  CJRVE1(1#1#0) 

538!  CALL  b0INT1(2O.,10.E-04) 

539!  CALL  ®0INTl(2#ll.O.E0> 

5*0!  CALL  CURVE1I2#0*3> 

5*1!  CAUL  T  jTLE2( 32» ‘  SU89PT  i*AL  GAJN!  <X  VS  Tr^E(SEC)*) 

5*2!  CALL  3RAPH1U1,#«TIHE*#10»*'  <X  •) 

5*3!  C 

5*4!  C  OATA  F8R  THE  SEVENTH  GRAPH  (<Y  OPTIMAL)  ^OLLiWS. 

545!  C 

546!  08  47  I »1# 51 

5*7!  X( I )«SAAV<  26# I ) 

5*8!  Y»RK8PT(2*I> 

5*9!  CALL  »0INT1U*X#Y) 

5501  47  CONTINUE 

551!  CALL  CURVEH  1*1*0) 

552!  CALL  P0INTl{2*0»#-10.0E-0*) 

553!  CALL  ®9INT1(2*11.»0.E0> 

55*!  CALL  CURV£1<2,0*3> 

555!  CALL  TITLES* 32, ' SUBOPTIMAL  GAIN!  <Y  VS  TTMEOEC)') 

556!  CALL  3RAPH1U1,#  »TIME**10»*  •  *Y  *) 

557!  C 

558!  C  OATA  FOR  THE  EIGHTH  GRAPH  (<Z  OPTIMAL)  FOLLOWS* 
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5591  C 

5501 

5611 

5621 

5631 

5661  6ft 

5651 

5661 

56?t 

568J 

569 1 

570 : 

571 :  c 

5721  C 

5731  C* 

5761 

5751 

5761 

5771 

5781  69 

5791 

5801 

5811 

5821 

5831 

5861 

5851  C 

5861  C 

587  5  C 

58ft  1 

5891 

5901 

5911 

5921  50 

5931 

5961 

5951 

5961 

6971 

5981 

5991 


09  68  !»1*51 
X(t)»SAAV(26*l ) 

Y»RK5BT{3*1> 

CAUL  »9!MTm*X»Yl 
CONTINUE 

CALL  CJRVEm»l#0) 

CALL  B91NT1<2»0««20*E-01> 

CALL  »91NTl(2«ll**3tE01 

gfct  5lTLKcIs5»WMPTI«4L  GAIN!  <Z  VS  TIISISECIO 
CALL  US5l(ll..*T!H6».ll)..*  <1  *> 

3 AT A  F9R  THE  NINTH  GRAPH  <<VX  9PTIMAL)  FOLLOWS. 

08  69  !»1*51 
X< 1 ) «SAAVt  26# 1 1 
Y«RX9BT <  6#  J  ) 

CALL  B9INT1<1*X#Y) 

CONTINUE 

CALL  C'JRVE1»1#1*0> 

CALL  »9INT1(2#0»#5*DE»0?> 

CALL  a9I'4Tl{2*ll*#»5«0E*O21 

CALL  TnSailv'suMPTiMAL  fiiy*  <vx  vs  tiheiseom 
call  graphiui.* 't:he'»id»*'  <''**> 

3ATA  F9R  THE  TENTH  GRAPH  KVZ  9PT1MAL)  p9lL0WS« 

OP  50  1*1*51 
X ( I  )«SAAV(26/ 1 ) 

Y«9X8»T<5» I ) 

CALL  B0I  NTl ( 1#  X#  Y> 

CONTINUE 

CALL  CJRVE1<1*1*3> 

CALL  f>9INTl(2*0*»20*OE-02) 

CALL  09INTl<2*U*»O»iO) 

“tt  ?iTuii<33?’Iia»PTi'**-  *»«  <«  vs  rnt.sKix 

MU  3WHt(U..'niE'.15..’  <V!H 
RETURN 


71 

it 

91 

lot 

lit 

1  s  t 

131 
141 
IS  t 
161 
1 7t 
18 1  C 
191  C 
20 1  C 
21 1 
22 1 
231 
24 1 
251  C 
261  C 
271  C 
281  C 
291 

so: 
an 
32!  C 
331  C 
34:  C 
351  C 
361 
37t 
sat 

39: 

40: 

411 

421 

43: 

44: 

45; 

46; 

47;  c 
48:  c  i 
49:  c  { 

so:  c  i 
sit  c 

52: 

531 

541 

55: 

561 
571  C 
58:  C  SI 
59:  c  Tl 
60 :  C  t! 

61 :  c 
621 


9*r'M<X*DX,5T> 

REAL*5  X(26J*DX(26) 

SS1S-,|1WB'* . 

Jf  ®J  A*1- AT#  DES#  aeSf ;  2f ;  SIdcltat,  I  C»AIMT#  I  ««AX«  I  U»DAT#  MS 

TM£Se  AR?  S9»1S  CONSTANTS  PAR  THIS  RjV. 

WS«GE/RE 

WX1«0.00 

ffi?»3S3R!gJ?:sj 

V-CT5'  Et*r 

wx«wxi 

WY»WVl 

W2»wzi 

pft9  TMIs’case!'12  3r,?rvArrv*S  ARE  2ERJ.  T- * rfX9»T»,OO»T«wZ55T«0 

F41  •arlS*  ( W  V»WY*WZ#W2 ) 

P42«.rtX*WY 
P43«-wX*w2 
P51 »»WX*W2 
F52«.wY*w2 

vPCf  !  ?°*WS* { WX#WX*«f  Y»WY ) 

Y2»X(?) 

Z3»X { 3 ) 

VX4«X(4) 

V25«X(5) 


« JWAT?5  9TAT?AMulfS4°Aef|?Vf^lfi^|,  AS  CJRRrvr 

TIME  integration  sequence.  ■  T  r  nST  AS<5  th,9USH  t3e 

DXtl )»VX4*or 

DX(2)OO0*0t 

0X(3)«VZ5OT 

SaH?  s^rsvjy 

CALL  RAN0J{ tx»  t Y.RNQMl > 
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63! 

64! 

65! 

66! 

67! 

681 

69! 

70! 

71! 

72! 

73! 

74! 

75! 

76! 

77! 

78!  C 
79! 

80! 

81! 

82! 

83! 

84! 

85! 

86! 

87!  C 
88!  C 
89!  C 
90! 

91! 

92! 

93! 

94! 

95!  C 
96!  C 
97!  C 
98! 

99! 
100! 
101! 
102! 
103!  C 
104!  C 
105!  C 
106!  C 
107! 
108! 
109!  97 
110! 
111! 
112! 
113! 
114! 
115! 
116! 
117! 
118! 
119! 
120!  C 
1  ?1 !  C 
122!  C 
1231 
124! 


CALL  RANDU<IY»tX#*NDM2> 

CALL  RAN0J<IX#IY#RNDM3> 

CALL  RAN0J(IY#lX#RNDM4> 

CALL  RANDJ<tX#lY#RN9M5> 

CALL  RAMDJ<  JY#  TX,RM0M6> 

CALL  RAN9J(IX*IY#RN0M7> 

CALL  RAN0J<IY#IX#RN3M8> 

CALL  RANDUdX#  IY#RN0M9> 

CALL  RANDJ<IY#tX,RN0M10> 

CALL  RANDU(IX,IY*RNOMli) 

CALL  RANDJdY# IX#RNDM12) 

CALL  RANOJdX#lY,RN0Ml3) 

CALL  RANDJdY,  IX#RN9M14> 

CALL  RAN0JdX,lY,RNDM15) 

CALL  RANDJdY#  TX#RNDM16) 

QX»1P53*D0»DSQRT(-2#D0*DL83(RNDM1> )*DC9S(P*D0*PI*RNDMP) 
3Y«13l7*D0*9S3RT(-2.00«DL93(RNDM3l  >OC9S(2.Do*Pl*RNDM4> 

02*  210*D0*DSQRT ( •  2#D0<*DL9G{RNDM5)  >*DC9S<2.D0»Pt*RNDM6> 

QVX«  1«200*DSQRT!-2.D0*Dl83<RNDM7>  >OC8S(2.DO#PI#RNDM8> 

3VZ»  l*4D0*9S0RTe2<.D0*’V.93(RNDM9)  >*DC9S(2.D0*PI»RNDM10) 

RX«  5000*9SQPT«-2.Do*’5L83(R'J5‘111  )  >*DC9S<2O0*PI*RNDMi2> 

RY«  50.D3*DSQRT(-2.D0*DL83(RNDM13))*DC9S»2O0*Pl»RNDM14> 

R2«  100*D0*0SQRT(»2*D0*9l8G(RNDH15)  1  *DC9S(2»D0*Pl*RNDM16> 

THESE  ARE  THE  PERTURBED  VARIABLES*  TC#  THEY  WILL  INCLUDE  N9ISE. 

XP6"X<  6 ) 

YP7«X(7) 

ZP8-X(8> 

VXP9.XC91 

VZP10«X(10> 

THESE  AR?  ArTJAL  STATE  EQUATIONS# IE*  THEY  ARr  P“RTJR9ED  WITH  N9ISE. 

OX ( 6 ) ■ ( VXP9+QX ) *0T 
OXf7>»(0.00+QY)»OT 
0X(8)»(VZB10*3Z)*0T 

0X(9)«(E41*XP6*-P42*YP74P43*ZP8-2.00»WY*VZP13A3VX)*3T 
OX ( 1 0 1 ■ <  E51 •XP6+F52*YP74P53*ZP84?.00*WY*VXP9+3VZ ) *DT 

THIS  IS  ”HE  r. MATRIX  C9MP0RrO  9F  ®ARTI ALS  7VALJATE0  434UT 

the  current  estimate*  that  is#  xhat  ■  xnrh+variatisn  xhat. 

OB  97  I3«l#5 

00  97  J3«l#  5 

F(I3. J3)«000 

F(l»4)»l«90 

F<3#5)»1*00 

F(  4#  1 ) »F41 

F(4#  ?) »F42 

F<4#3)"F43 

F(4#S)*»2«00*WY 

F(5.1>»E51 

F(5#?)»F52 

F(5#31«F53 

F(5#41«42*00«WY 

COLLECT  THE  C8VARI ANCE  MATRIX  ELEMrviTS  FR0M  THE  STATE  VECT9R. 

00  98  I2»l#5 
08  98  J2*1#S 
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1251 

1261 

1271 

1281 

1291 

1301 

1311 

1321 

1331 

1341 

1351 

1361 

1371 

1381 

1391 

1401 

1411 

1421 

1431 

1441 

1451 

1461 

1471 

1481 

1491 

1501 

1511 

1521 

1531 

1541 

1551 

1561 

1571 

1581 

1591 

1601 

1611 

1621 

1631 

1641 

1651 

1661 

1671 

1681 

1691 


98  P( 12. J2)*0.00 
KlilO 

DO  13  I -1#  5 
09  1?  J*1.I 
KliKl+l 

p<j#n*x«i> 

12  P(!«j)'P<j«n 

c 

C  P§RM  MAT91X  pp. 

C 

00  99  11*1.5 
00  99  Jl*l.5 

99  PP(!1# J1)*0»00 
00  103  X  «1#  5 
00  100  J*1.5 
DO  103  <*1.5 

100  PP( ! . J) *PP( I.  J1  *P(  I#tO**(  <.  J) 

C 

C  P0RM  MATRIX  ELEMENTS  OP  D*3*DTRANSP0SE 
C 

00  96  14*1*5 
00  96  J4*l»5 
96  DQDT( !4. J4)*0.00 

31*1253.00 
011*31*01 
OOOT ( 1. 1 )*3ll 
02*1317.03 
022*32*02 
OOOT <2. 2) *022 
03*213.00 
033*03*03 
0Q0T(3*3)*333 
04*1.203 
Qit*aQ4*Q4 
D03T14.4)*344 
35*1.403 
055*05*05 
DQ0T<5.5>*355 
C 

C  COLLECT  THE  ERROR  COVARIANCE  EQUATION  POR.  INTEGRATION  IN 
C  THE  STATE  VARIABLE  COLUMN. 

C 

K2*10 

00  106  M*l# 5 
DO  106  N*l# M 


1701  K2*K24l 

1711  106  OX(K2)*(PP(M#N)+PP<N#M)OOOT(M,N))*OT 
1721  C 

1731  C  THE  LAST  STATE  Is  TIME  NEEDED  POR  THE  INTEGRATION  SUBROUTINE. 
1741  C 

1751  DX126)»1.00*DT 

1761  RETURN 

1771  END 
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It 
21 
31 
41 
St 
6t 
7t 
St 
91 
101 
ut 
12: 

13: 

14: 

is: 

16: 

17: 

is: 

19: 

20 : 

?n  no 
22* 

231 

241 

25!  Ill 
26: 

271 

281 

291  112 

301 

311 

321  113 

331 

341 


SUBR»jmE  RUK(XR/0T/N> 

HpL!CJT  REAL**  fA*H/B»?) 

08'JBlE  PRECISION  OT 

OIMEMSISN)  XR(36),UlC26),ci(2i)#01(26) 
1CPSAa?(15?Jp(5?5?V<26'5,,<SAAVJ°(?6,#><SAAV(5,# 

1  RK9PT(5/58)/PPLuS(25)/X'lSAVt5) 

CBMM9M/ARRA Y3/£RRX<  5  > / EpRXM ( 5 ) 

C0MM9\t/ARRA  Y4/F  <  5*  5  ) ,  FP<  5*  5  >  *  MOT  f  5*  3 ) 

CBMMBM/Bl/Xi/ Y2#  Z3i XP6i 7*7/ Zp8/ RX/  RY. RZ 

C8MM»\l/C2/lxfLlHCLAT,0ES'3E#,‘#P,*T':,'lT*T*IC9JVT*I'lAX*IU'*9AT,'JS 

CALL  OlFTEfltXR/Dl/OTJ 
08  110  I*1/\J 

Uim.XRf  I  Uo. 500*01  (!) 
call  3!FFE3(U1/Fi/DT> 

08  111  I*1,m 

oim*oi(n*2.*)0*Fim 
U1 ( I )*XR{I )+0»5D0*Pl < I ) 
call  0IFTE3tUl.Fl/0T) 

08  112  1 *1/ M 

oi(n*oiii)*2.oo*Fitr) 
ui(n*xRn)*Fim 
CALL  OIFFE3tUl/Fl,0T) 

08  113  I-l/M 

XR(  n •XRU)*<01  (I)*F1  (I))/5, 00 

RETURN 

ENO 
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/l 


78 
88 
98 
108 
118 
128 
138 
148 
158 
168 
178 
188  C 
198  C 
?08  C 
?1 8  C 
228  C 
238 
?4  8  3 
258 
268 
278 
288 
298 
308  2 
318 
328 
338  1 
348 

358  15 

368 

378 

388  16 

398 

408 

418 

428 

438 

448 

458 

468 

478 

488 

498 

508  20 

518 

528 


?MPL? Z I rN!rAL28 *  ^ '*«» ^  ^A.  VMB»  MVC) 

DI9EM5I9'J  A(WA>*B<W3>,C(MVC) 

.”3«m""!5^'''56'5','s“VJO,S6l'xs‘«'5'' 
C01M9V/A33AY3/E9RX  ( 5 ) *  E33X9 (5 ) 

c;^v^s;;a,T'0ESi3E',E',,''T'^Lr«T.I=,jvT, 

WERE  J«l  »>  C*A9 
J«?  ■>  C»A0T 
J»3  •>  C«AT9 

G8  T9  (1*2*31* J 

MA.NCA 

MB*MC3 

mc»n5a 

MD*1 

ME«1 

SR  T9  16 

90»Mi?3 

GR  T9  15 

MB«MC9 

M0«1 

HAaNQA 

MC-NICA 

MEaNtJA 

0»  20  !■!* MA 

NCaI.9A 

NB.O 

OB  20  K»l»v19 
NCaMC+MA 
C(VJC)a3.03 
MA-I-mA 

IT  (J.E3.3)  VAa<I«l)*MC 

IE  (J.E3.2)  MBaK-^B 

OB  2o  L«1*9C 

NAaNA+ME 

NBaM3*M0 

C(NC>aC(NC).fA(VA)»BtNB) 

RETURN 

END 
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'v  iwiwr'  ’-v^rr 


Mil  i,u»p*ugiij,  jj  ■ 


It  C 
21  C 
31  C 
*1  C 

St  c 

61  SUBR1JTIME  RAWUIIX^IYaYFU 

7!  REALM  YFL 

8:  IY»!X*65539 

91  IF  < 1 Y )  5«6i6 

10:  5  I Y«JY+21 474 83647+1 

111  6  YFL*fY 

121  YFL» YFL* • 465661 3E* 9 

131  RFTURM 

141  EN3 


11  C 
21  C 
31  C 
41  C 
51  C 

61  ''L^CX  "ATa 

7i  implicit  realm  <A-i~iB.z) 

81  C''M“9*/ARRAV1/!;AAV'<261«8>  j<3aAVUP(26)aX«5AAV(5Ia 

91  1  PSAAVUBl.PlSiB) 

Id  c«muKN/ARRAY2/hr'W1  (1a5)#HR«‘.i2(1*5>*HR6w3(1*5)>HC5)*8TR(5>» 

111  1  RK'iPT(5*5‘U/P»L'JS125)AXWSAV(5i 

121  CH^veN/ARRAV3/FRRX(5)»FR«X‘1C5) 

13!  CnMMtiN/ARRAY4/F(«i#5)AFP(5/5)#Of3RTl5#5) 

14j  C"M^eN/Rl/XliY?/Z3AXP6/VP7»ZR«ABX#RY*RZ 

15!  CA'iMHN/Cl/PMEGA/EUAT»'>r(5*C?E/RE/Pl#T#DELTAT/  ICBUNT#  IMXXj  IUPDATaNB 

161  Cn«YWN/C?/IX#LIf‘ 

171  HaT A  SAAV/15CA*O.OP/»F/?S#r.OO/,P/25«O.PO//FP/25iO.OO/* 

18!  1  M3T/?5*O»DO/,HMe:'.l/5*C«D0/»HRftW2/5*0.D0/# 

ig.  a  RA AVUP/?A*0*00/aXSAAV/5*0»00/#oSA AV/15*0»(10/#PPUUS/25*0»00//ERRX/ 

20!  3  5*C*Or/,Er'iRX,VB*o.OO/iFTBAftKBPT/5*OtD0.290#0»00/#HRAW3^5*0«00/ 

21!  4  #XKS*V/5*1«!3C/ 

221  ENO 


150- 


cm 

© 

o 

© 

© 

o 

o 

o 

o 

o 

o 

o 


N 

N 

a 

UJ 

Nl 

Nl 

> 

> 

CM 

c 

o 

© 

o 

G 

o 

o 

o 

© 

o 

© 

H 

••  o 

• 

• 

O 

LL  O 

h 

1} 

<  H 

a 

O  Uj 

X 

a  x 

> 

D  > 

•• 

fc-» 

2 

UJ  o 

UJ 

h 

X 

<  C 

Uj 

H  © 

Of 

in  © 

D 

o 

m 

o  o 

< 

UJ  D 

UJ 

CD  O 

X 

Of  O 

D  O 

< 

H  m 

a 

Uj  © 

UJ  • 

Of  • 

a  o 

oo 

U. 

UJ  H 

UJ  n 

X 

CO 

»-  a 

UJ 

••  PM 

-  N, 

UJ 

M 

UJ 

H 

o 

< 

»- 

o  o 

V) 

N-  o 
<  o 
o  o 

►*  O 
>  o 
*1  o 
2  h* 


c 

a: 

a 

u. 


m 

o 

o 

CM 

GO 

m 

oo 

<6 

(A 

in 

in 


u 

UJ 


CM 

o 

Q 

m 

in 

O' 

o 

£ 

O' 

00 

oo 

oo 

* 

«  © 
t 

UJ  II 
H-  Uj 
<  X 
C.  > 
a 
d 

* 


u.  © 

x  * 

VJ  >o 

Of  N 

d  o 
to  co 
< 

111 

X  *-* 

_  o 

<  CO 

• 

OC  w 
UJ 

H  II 

u. 

<  UJ 

PM 

UJ 

H- 

<  o 

h 

</>  O 
r* 

CM 
Q.  IT. 

c  co 
a  N» 
0  cm 
u;  cm 
«M 


-j  co 

Cl. 

of  a> 

<  «~4 

c  © 

O  |w 

H 

H  • 

h  • 

H  O 

<  © 

<  O 

Of 

c 

O  1 

UJ  l( 

H 

N4  H 

II 

2 

> 

> 

X 

*-•  a. 

<  Uj 

<  Uj 

UU 

o 

w* 

2  > 

2  >■ 

2 

X 

_J 

mi 

** 

H  f 

< 

<  © 

_  w 

M  M 

> 

2 

K 

< 

N»  O 

Of 

of  a 

< 

o 

UJ 

IU  CM 

00 

0  © 

2 

2  <n 

C  © 

Of  o 

—  >* 
w 

Of  o 

O 

O  0 

UJ  © 

UJ 

UJ  © 

co 

H 

k  m 

-j  in 

< 

<  <c*. 

<  CM 

X 

x  m 

D  « 

m  a 

*-<  f* 

h  • 

» 

H  * 

o 

oo 

in  o 

in  o 

• 

< 

u. 

Uj 

o 

II 

N 

N 

Uj 

UJ 

UJ 

x  a 

X  Uj 

X  Uj 

M 

N 

H  X 

h-  X 

*-  X 

«G 
o  o 
a.  o 

DO 
& 
h*  J% 
2-4 
UJ  • 

X  o 
UJ  I 
0  II 
DM 
40  Of 

<  a. 
UJ  u, 
X 

IU  4 

xo 

c 

UJ  © 

0  o 

C  D 
u.  o 
UJ  o 
AC  © 

n- 

UJ  «-4 

^  Pi 

<  *-« 

cl 

£  > 

n-  a 

<a: 

X  LU 


CO  M 
UJ  © 

2  O 

o 
cr  © 
c  © 

Of  © 

a  © 

U-  <o 

in 
of  cm 
uj 

K  • 
-J  O 
m  | 
u.  II 

X 
Uj  Of 
X  a. 
K  UJ 


CM 

CM 

o 

o 

* 

o 

O 

o 

o 

in 

1 

o 

o 

a 

O' 

o 

o 

0 

© 

© 

in 

c 

o 

o 

© 

o 

o 

Ov 

o 

o 

O 

c 

O' 

9 

• 

o 

• 

c 

OOOOH 

1 

•  •  •  •  « 

N 

M 

o  o  o  ©  o 

K| 

Nl 

> 

> 

M  H  N  M  N 

Of 

0 

af 

Of 

in  m  in  m  m 

UJ 

UJ 

•m  cm  cn  ^  in 
o.  a  a.  a  a 

CM 

m 

O 

o 

«4* 

O 

O 

a 

1 

© 

N* 

a 

© 

CM 

© 

o 

<0 

o 

G 

CM 

© 

O 

M) 

o 

o 

0 

o 

o 

o 

Cj 

© 

o 

o 

co 

© 

CM 

o 

« 

• 

©  O  O  #-4 

Cj 

© 

till 

1 

1 

o  o  o  o 

It 

K 

X 

X 

MM 

> 

> 

Of 

Of 

*  4-  *  * 

Of 

0 

M  N  Pi  M 

UJ 

UJ 

a.  a  a  a 

* 

* 

in 

c 

© 

o 

«*  o 

••  c 

o 

o 

0 

o 

ui  o 

UJ  CM 

o 

<  co 
Q  CO 
CX 

D  4 

in 

t  *0 

2  CM 

u-  • 

X  O 

UJ 

Of  II 

D  nj 

CO  Cb 

<  a: 

UJ  UJ 

X 

UJ  CO 
X  o 
H- 

C 
Of  O' 
UJ  M> 
i-  »o 
U.  O' 

<  <0 
H 
© 

Uj  CO 
N-  CM 

<  4) 
h  • 
</>  C 

c  1 

UJ  > 

H  Of 

<  Of 

X  uj 


</>  >* 

uj  c 

z  a 

~  r* 

H 

a  in 

O  cr. 
of  in 
Of  O' 
Uj  CM 
H 

Of  o 
UJ  #M 
K  • 

-J  o 

•*  I 

u.  M 
X 
UJ  of 

x  of 

H  UJ 


C 

c 

© 

o 

o 

o 

© 


in  ms  m  4-  *• 

o  ©  o  o  © 

cocoa 

N  O*  H 
CM  CM  00  K  00 


r-  ©  o  © 
cm  cn  ©  0  cm 

<-4  0  0  <M  O 

hO'Mhim 
n*  0  -4  in 
cm  in  r-  0  o 

<OMhO(M 

IfirtNPIO' 

•  •  •  •  • 

oo  oo  o 

1 

M  N  e  H  N 

m  m  m  m  in 

o 

• 

w  cm  m  *  in 

o 

a.  a.  a.  a.  a 

H 

Nl 

0  in  0  in 

> 

o  oo  o 

X 

o  o  o  o 

C  4  Mp4 

O'  o  n-  n- 

0  *  0  CM 
m  m  <-«  ao 

in  m  ©  cn 
©  nj  ©  ^ 

0  0  O'  O 
h-  ©  CM  CM 

CM  ©  fH  CM 

CM  CM  CM  CM 

O  O  O  © 
I 

H  N  H  M 

m  cm  m  4- 

a.  a  a  a. 


•t  in  o 

o  o  o 

o  c  o 

CO  CM  CM 
O'  *  CM 
O'  CM  CM 
0  0  CM 
4-  *  N- 
fO  O*  «* 
H  N  ^ 

co  n* 

O'  « 


H 

X 

> 

X 


O  O  -4 

••  h-  n  © 

u#  •  •  • 

•  •  • 

K  O  OO 

UI  ©  o  o 

< 

h  | 

© 

o  II  II  || 

<  II  u  II 

• 

a 

o 

G 

D  co  cn  co 

a  co  co  co- 

•-4  CM  CO 

D  «-4  CM  cc» 

II 

h  O.  (X  Q. 

0  a  a. 

2 

h 

fM 

111 

2 

X 

2  m 

uj  in  n 

uj  o 

x  o  o 

0 

UJ 

D  C 

0  o  o 

wi  O 

D  CM  © 

<  O 

V)  ©  © 

Uj  O 

<  in  © 

X  o 

UJ  0  CM 

•• 

O 

X  CM  * 

<  w 

^  M 

O 

<0  4* 

2 

o 

00 

u.  o 

M* 

< 

0  C  ^ 

0  *4  © 

O 

C  •  4 

U  t  • 

u.  O  © 

HOC 

0  C 

UJ 

U 

ii:  • 

0  II  II 

<  II  II 

H-  O 

X  CM  CM 

X  CM  CM 

-j 

•>4  H 

CM 

•-*  CM 

L 

0  a  a 

a  a  a 

> 

K 

»- 

C  X 

< 

< 

2 

X  IT. 

IN 

o 

O 

> 

U. 

lu 

u, 

O  Cl 

o  a 

< 

2  C 

2  CO 

> 

<  © 

<  © 

1 

r 

**  m 

u 

0  © 

0  H 

X 

^  © 

<  Or 

M4 

>  o 

>  N- 

H 

c  © 

1  C  0 

o  o 

O  'f 

•J 

o 

< 

0 

0  CO 

X 

D  • 

c  • 

M  © 

0  O 

0  C 

H  • 

0 

0 

a.  o 

Uj  K 

Uj  N 

c 

Uj  ^4 

UJ 

H 

Ui 

X  •-» 

X  -i 

X  X 

h*  0 

H*  a 

K  X 

•  151- 


r 


o 

o 


H 

h 


8 

A 

IN 

in 

f*. 

N 

in 

H 

o 

H 

a 


m 

o 

© 

>o 

♦ 

o 

<0 

m 

4- 

•o 

<o 

in 

o 

I 

H 

a 

x 

> 


* 
Ui  © 
0 

<  © 

h  ^ 

in  4> 

nj 
O  4 
UJ  o 

cd  a 

OC  #N 

©in 
0  0 
a  CD 
UJ  • 

a.  © 

0  tt 

0  a 


*  m 

© 

go 

0  IT 
<  0 
O  «M 

*-  c 
*  ** 
2  CNI 


O  w 
a  4 
0  <0 
UJ  O' 

o 

-i  CD 

<  m 
Dm 
h  • 

u  © 

«< 

N 

UJ 

x  a 
0  x 


m 

© 

o 

in 

in 

N 

>0 

o 

o 

oo 


© 

N 

UJ 

N 

> 


4 

o 

o 

m 

co 

<0 

o 

* 


nj 

♦•  NJ 


0  I 
<  M 
O  Ui 

a  x 

D> 


x  in 

UJ  o 

D4> 
tA  * 

<  NJ 
UJ  * 

x  in 
c* 

<  N 

0 

Ui  r-4 

0  i 

£° 
0  II 
0 

Ui 


0 

h  IN 
<  o 
0 

m  o 
o 

O' 

or  m 

o* 

£$ 


N 

m 

-J  IT. 

<  m 
•.  • 

N 
0  C 
©  m 

h  • 

IT* 

N*  © 

<  C 

H 

0  1 

0  H 

z 

c  | 
>H 

X 

•N  0 

<  0 

0 

C 

ui1- 

z> 

X 

x 

•N 

<  *4" 

> 

z° 

pC 

< 

< 

ft 

N 

a  © 

0  Nj 

m 

0  0 

Z  0 

in 

0 

Q* 

UJ  CO 
h  n 

<  in 

z  0 

r* 
K  # 
in  o 

uj  Y 

M 

UJ 

XUJ 
0  X 


m 

G 

O 

in 

m 

O' 

in 

•M 

M 

O' 

m 

in 

© 

i 

H 

UJ 

3 


4- 

O 


0 

o 

4 

© 

nj 

m 

m 

• 

••  o 

U.I 

0  a» 
<  x 
a  > 
a 


a*  © 

x  in 

Uj 

0  O' 
D  m 

in  -» 
<  ® 
uj  o* 

x  a- 

nj 

<  in 

• 

a  O 

UJ 

0  II 
U. 

<  u. 


I-  nj 

<  O 
0 

in  c 
•*  4 
G 
0  m 
O  CM 
0  IN 

0  e 
uj  © 

_  © 
0  IN 
©  N 
0  • 

<  c 

©  I 

>  11 

<  0 
z> 


<  in 


3 


UJ 

O  5 

Uj  i-« 
U  CN 

<  m 

X  IN 


m  G 
UJ  | 

0" 
X  Ui 
0  X 


* 

o 

o 


0 

o 

1*1 

4 

in 

m 

4 


O 

I 

It 

NJ 

> 

0 

0 

UJ 


4 

© 


0 

4 

O' 

<£ 

O 

G 


© 

I 

M 

X 

> 

0 

0 

UJ 


4 

o 


••  o 
in 
uj  c 
0  * 

<  0 

C  in 

S2 

K* 
2*  0 
0  • 
X  © 
UJ 

0  II 
D  Kl 

ina 

<  0 
UJ  UJ 
X 

u«  m 

5° 

O 

Ui  in 

a 

O  N 
0  O' 
tu  0 

0  Q 

in 

0  IN 

5S 

So* 

Q  II 
U-> 

0  0 
<  ** 

X  UJ 


in  * 

0  G 
SC 

i-*  <\j 

N 

a  m 
c  4 
0 

0  0 

0  IN 

m 
0  O' 
UI  O' 
0  • 
«J  G 

r  i 

£S 


4 

O 


m 

in 

in 

& 

0 

0 

O' 


G 

I 

N 

NJ 

> 

0 

0 

0 


* 

O 

© 

0 

0 

m 

0 

m 

O' 

in 

m 

o 

nj 

• 

G 

I 

N 

X 


£ 


in 

o 

••  © 
m 
uj  c 

SS 

hS 

Z-4 
U  t 
X  o 

0 

0  II 

D  NJ 

m  a 
<  0 
Uj  UJ 


0  m 
X  © 

o 

0  O' 

UJ  ao 

nj 

XL  o 

<  N- 

r** 

m 

0  0 
H  0 

<  «H 

h  • 

me 

©  H 
0  > 
H  0 

<  0 
X  0 


m  u\ 
0  © 

20 
•m  m 
in. 

SP 

a 

0  N 
IN 

0  m 

0  «H 

0  • 
0  o 

f-  i 
U.  II 
.  X 
0  K 
X  0 
»-  0 


-152- 


•  00NN 

OOOOO 

OOOOO 

NION  NO 

m  o  in  m  m 
0  c  m  N>  K> 
m  in  m  m  0 
m  in  0  0  in 
m  m  I? 0 

O'  O  K  *  IN 
nj  0  in  K 
m  ©  o  0  m 

•  •  •  •  • 

OOOOO 

H  N  N  M  N 

ia  tft  in  in  m 

HNmilA 

a  a  a  a  a 


o  o  o  o 
0000 

■i  •*  m>  m» 

SK  4  H 

k  o  o  a 

(SI  CO  u\  N 
m  if.  n  m 
in  >r  o>  00 
noon 

.... 

OOOO 

U  H  H  N 

-i  *  *  -t 
S*  N  m  4- 

o.  a  a  o. 


CONS 

o  o  o 

o  0  c 

K  (A  CP 
»!•  O'  AJ 
C  K  . 
w  r»  o> 
^  c  co 
ova 

NlA  O' 
■»  m  i*> 

••  O  O'  sj 

"•  "i  ro 
u-  .  •  • 
J-ooo 
S  1  1 
O  N  ||  H 

Snnic 

.  <v  <n 

k  a  o.  a. 
2 
0 

X  N  © 

0QO 

goo 

</>  O  A- 
CBIC 

0  «N  0 

x  o*  in 

O  NJ 

<ON 
O'  ^ 

0 

0  0  M 

O  •  . 

0  w  C 
0 

0  It  M 

X  Nj  in 

NJ 

0  0  0 
0 
< 

X  0 

o 

0 

uo 

z  0 

<  m 

X  $ 

<  m 

>  co 

C  <M 
O  « 

c 

Ct  <0 

c  • 
at  O 
cc 

ui  n 

ui  w 

x  M 

1-  CL 


<0  K  O  •  O 

-1  CM  mi  M 

OOOOO 
min  cam* 
n  n  m  m  <0 

CD  CCr  V*  «0  0 
1A  1  IA  ■  O' 

O'  N  B  O'  N 
n-ciaiao 

IA  m  0  CD  M 
IA0N  mCD 

o  "•  O  o  m 
0  N  H  M  IA 

N  Ml  t 

(A  IA  IA  0  IA 
HNKIilft 

&  a  a  &  a 


0  0  O'  N 

HidH 

O  Ci  O  o 
cd  cor- o. 

co  IA  (d  0 
N-  IA  (SI  Ai 
rtCD.O 
p  ia  m 

0  ^  m  o 

»CNO 
r'K(f»K 
AI  O  O  IA 
AI  CO  0  A* 
.... 
oooo> 

I 

N  N  M  H 

0  0  0  0 

-*  NWI0 

a  a  a  a 


a  03  h 

"HM 

000 

0  0  Al 
0  0  CO 
«IAN 

A’  t|  A 

0  K  « 
"d  (A  -• 

•.on 

-.00 

AI  m.rd 

»•  <*  0  AI 
ui  C  O  O 

Si<« 

S  m  m  m 
D«n  in  m 
000 


uj  in  in 

X  H  H 

Ui 

0O© 
D  O'  NJ 

m  o  nj 

<  -4  O' 
Uj  ~4  <£ 

x  4*  m 

0  0 

<  41  ^ 
NJ  IN 

0  NJ  0 
0  •  • 
c  © 

0 

<  «  M 

X  IN  NJ 
►•HNJ 
0  0  0 


X  4 

N 

0 

u  O 

5<G 

3^ 

5  m 

0 
0  0 
c  • 
0  o 
0 

0  M 

0  ~4 

Xn 

k  a 


o 

I 

8 

§ 

o 

o 

o 

o 

8 


N 

NJ 

> 

X 


o 

o 

G 

© 

G 

G 

O 

O 

o 

o 

in 


© 

1 

tt 

x 


o 

o 

© 

o 

© 

o 

o 

o 

o 

o 

G 

o 

0 

t 

o 


NJ 

o 


c 
o 
© 
in  0 
2  G 
N  C 
<© 
©  © 
'O 

0  m 
0  • 
H  C 
-J  I 
••  N 
0 

> 

N 

>  m 

5* 

t  O 
I  o 
;  o 

N4  Q 

K  o 
© 
-J  o 

23 

St 

& 

e  1 

N 

Sk 

K  M 


REFERENCES 


1.  Broxmeyer,  C.,  "Damping  an  Inertial  System,"  ARS  Guidance  and 
Control  Conference,  Stanford  University,  Stanford,  California, 

7-9  August  1961. 

2.  Duncan,  D.  B.,  "Combined  Doppler  Radar  and  Inertial  Navigation 
Systems,"  Proc.  National  Electronics  Conference,  Vol.  14,  1958. 

3.  Dworetsky,  L.  H.,  and  Edwards,  A.,  "Principles  of  Doppler- Inertial 
Guidance,  ARS  Journal.  Vol.' 29,  No.  12,  December  1959, 

pp.  967-972. 

4.  Friedman,  A.  L.,  "The  Use  of  Speed  and  Position  Fix  Information 
in  Inertial  Navigators,"  ARS  Guidance.  Control,  and  Navigation 
Conference ,  Stanford  University,  Stanford,  California, 

7-9  August  1961. 

5.  Broxmeyer,  C.,  Inertial  Navigation  Systems.  McGraw-Hill  Book 
Company,  New  York,  1964. 

6.  Sorenson,  H.  W.,  "Least- Squares  Estimation:  From  Gauss  to 
Kalman,"  IEEE  Spectrum,  July  1970. 

7.  Kalman,  R.  E.,  "A  New  Approach  to  Linear  Filtering  and  Prediction 
Problems,"  Journal  of  Basic  Engineering,  Vol.  82D,  1960, 

pp.  35-45. 

8.  Kalman,  R.  E.,  and  Bucy,  R.  S.,  "New  Results  In  Linear  Filtering 
and  Prediction  Theory,"  Journal  of  Basic  Engineering.  Vol.  83D, 
1961,  pp.  95-108. 

9.  Sutherland,  A.  F.,  Jr.,  and  Gelb,  A.,  Application  of  the  Kalman 
Filter  to  Aided  Inertial  Systems,  The  Analytical  Sciences 
Corporation,  October  1967,  Report  No.  TR-134-1. 

10.  O'Donnell,  C.  F.,  Editor,  Inertial  Navigation  Analysis  and  Design, 
McGraw-Hill  Book  Company,  New  York,  1964. 

11.  Pitman,  G.  R.,  Editor,  Inertial  Guidance.  John  Wiley  and  Sons, 
Inc.,  New  York,  1962. 

12.  Fernandez,  M.  and  Macomber,  G.  R.,  Inertial  Guidance  Engineering, 
Prentice-Hall,  Inc,,  Englewood  Cliffs,  New  Jersey,  1962. 


-153- 


juhhph  wyinn1,1!. 


13.  Markey,  W.  R.,  and  Hovorka,  J.,  The  Mechanics  of  Inertial  Position 
and  Heading  Indication,  John  Wiley  and  Sons,  Inc.,  New  York,  1961. 

14.  Lange,  B.,  Inertial  Guidance  Systems,  Stanford  University, 
Stanford,  California,  Spring  1967  (Claris  notes  in  Course  AA  272b). 

15.  Pinson,  J.  C.,  "Inertial  Guidance  for  Cruise  Vehicles."  Guidance 
and  Control  of  Aerospace  Vehicles.  Chapter  4,  C.  J.  Leondes  - 
Editor,  McGraw-Hill  Book  Company,  New  York,  1963. 

16.  Brock,  L.  D.,  and  Schmidt,  G.  T.,  "General  Questions  on  Kalman 
Filtering  in  Navigation  Systems,"  Theory  and  Applications  of 
Kalman  Filtering.  C.  T.  Leondes  -  Editor,  AGARD,  No.  139, 

February  1970. 

17.  Martin-Marietta  Corporation,  Tactical  PERSHING  Accuracy  Analysis. 
Report  No.  OR  9195-1,  April  1968. 

18.  Wiley,  C.  A.,  "Airborne  Radar  Navigation,"  Avionics  Navigation 
Systems,  Chapter  8,  M.  Kay ton  and  W.  Fried  -  Editors,  John  Wiley 
and  Sons,  Inc.,  New  York,  1969. 

19.  Stauffer,  F.,  "Airborne  Navigation  and  Ground  Surveillance," 
Airborne  Radar.  Chapter  14,  G.  Merrill  -  Editor,  *).  Van  Nostrand 
Company,  Inc.,  New  York,  1961  (In  the  series  Principles  of 
Guided  Missile  Design) . 

20.  DeBra,  D.  B.,  Private  Communication,  July  1971. 

21.  Bryson,  A.  E.,  Jr.,  Optimal  Estimation  and  Control  Logic  in  the 
Presence  of  Noise,  Stanford  University,  Stanford,  California, 
Winter  1971  (Class  notes  in  Course  AM  235b). 

22.  Bryson,  A.  E.,  Jr.,  and  Ho,  Y.  C.,  Applied  Optimal  Control, 
Blasdell  Publishing  Company,  Waltham,  Massachusetts,  1969. 

23.  Kaminsky,  P.  T.,  and  Doerer,  H.  T.,  A  Simplified  Derivation  and 
Engineering  Application  of  the  Kalman  Predictor.  Filter,  and 
Smoother,  Air  Force  Missile  Development  Center  Working  Paper, 

MDSC  EP-69-2,  November  1969. 

24.  Aoki,  M.,  and  Li,  M.  T.,  "Optimal  Discrete-Time  Control  System 
with  Cost  for  Observation,"  IEEE  Transactions,  Vol.  AC  14,  No.  2, 
April  1969. 

25.  Eppler,  W.,  and  Willstadter,  R.,  "A  Simulation  of  Map  Correlation 
for  Ballistic  or  Orbital  Weapons  Terminal  Correction," 

Transactions  of  the  Seventh  Symposium  on  Ballistic  Missile  and 
Space  Technology.  Vol.  II,  U.  S.  Air  Force  Academy,  Colorado 
Springs,  Colorado,  13-16  August  1962. 


154- 


26.  Develet,  J.  A.,  Jr.,  "Performance  of  a  Synthetic-Aperture  Mapping 
Radar  System,  IEEE  Transactions.  Vol.  ANE-I1,  No.  3,  September 
1964. 

27.  Diamantides,  N.  D.,  "Quantization,  Statistics,  and  Matching  of 
Maps  and  Pictures,"  IEEE  Transactions.  Vol.  ANE-11,  No.  3, 
September  1964. 


155- 

l 


